jeka/MyOwnEtherCATDevice
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Hakan Bastedt
2024-03-23 18:02:38 +01:00
parent 2df0a0980c
commit 369a795ce5
3 changed files with 86 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Firmware/include/StepGen3.h
View File

@@ -74,6 +74,8 @@ public:
volatile int step_type[MAX_CHAN] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // stepping types for up to 16 channels volatile int step_type[MAX_CHAN] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // stepping types for up to 16 channels
char *ctrl_type[MAX_CHAN] = {0}; // control type ("p"pos or "v"vel) for up to 16 channels char *ctrl_type[MAX_CHAN] = {0}; // control type ("p"pos or "v"vel) for up to 16 channels
volatile int user_step_type[MAX_CYCLE] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // lookup table for user-defined step type volatile int user_step_type[MAX_CYCLE] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // lookup table for user-defined step type
uint32_t stepPin[MAX_CHAN] = {0};
uint32_t dirPin[MAX_CHAN] = {0};
stepgen_t *stepgen_array = 0; stepgen_t *stepgen_array = 0;
@@ -88,7 +90,7 @@ public:
volatile double recip_dt; // recprocal of period, avoids divides */ volatile double recip_dt; // recprocal of period, avoids divides */
StepGen3(void); StepGen3(void);
void test(double pos_cmd); void updateStepGen(double *pos_cmd);
int rtapi_app_main(); int rtapi_app_main();
int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode); int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode);
void make_pulses(void *arg, long period); void make_pulses(void *arg, long period);
@@ -143,7 +145,9 @@ private:
// For the example // For the example
#define BASE_PERIOD 50000 #define BASE_PERIOD 50000
#define SERVO_PERIOD 1000000 #define SERVO_PERIOD 1000000
#define JOINT_0_STEPGEN_MAXACCEL 520.0 #define JOINT_X_STEPGEN_MAXACCEL 520.0
#define JOINT_0_SCALE 200 #define JOINT_X_SCALE 200
#define JOINT_Z_STEPGEN_MAXACCEL 520.0
#define JOINT_Z_SCALE 80
#endif #endif

35
Firmware/src/StepGen3.cpp
View File

@@ -955,8 +955,7 @@ int StepGen3::export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mo
} }
printf("Exporting %d\n", num); printf("Exporting %d\n", num);
// *(addr->phase[STEP_PIN]) = 0; // Several variants possible
// *(addr->phase[DIR_PIN]) = 0;
/* set default parameter values */ /* set default parameter values */
addr->pos_scale = 1.0; addr->pos_scale = 1.0;
addr->old_scale = 0.0; addr->old_scale = 0.0;
@@ -1086,24 +1085,33 @@ StepGen3::StepGen3(void)
step_type[0] = 0; step_type[0] = 0;
ctrl_type[0] = (char *)malloc(2); ctrl_type[0] = (char *)malloc(2);
strcpy(ctrl_type[0], "p"); strcpy(ctrl_type[0], "p");
stepPin[0] = PA11;
dirPin[0] = PA12;
step_type[1] = 0;
ctrl_type[1] = (char *)malloc(2);
strcpy(ctrl_type[1], "p");
stepPin[1] = PC9;
dirPin[1] = PC10;
printf("Hello World\n"); rtapi_app_main();
(void)rtapi_app_main(); stepgen_array[0].enable = 1;
printf("Hello World2\n"); stepgen_array[0].pos_scale = JOINT_X_SCALE;
stepgen_t *step; stepgen_array[0].maxaccel = JOINT_X_STEPGEN_MAXACCEL;
step = &(stepgen_array[0]); stepgen_array[1].pos_scale = JOINT_Z_SCALE;
step->enable = 1; stepgen_array[1].maxaccel = JOINT_Z_STEPGEN_MAXACCEL;
step->pos_scale = JOINT_0_SCALE; stepgen_array[0].enable = stepgen_array[1].enable = 1;
step->maxaccel = JOINT_0_STEPGEN_MAXACCEL;
} }
void StepGen3::test(double pos_cmd) void StepGen3::updateStepGen(double *pos_cmd)
{ {
for (int i = 0; i < num_chan; i++)
{
stepgen_t *step; stepgen_t *step;
step = &(stepgen_array[0]); step = &(stepgen_array[i]);
step->pos_cmd = pos_cmd; step->pos_cmd = pos_cmd[i];
update_pos(step, SERVO_PERIOD); update_pos(step, SERVO_PERIOD);
update_freq(step, SERVO_PERIOD); update_freq(step, SERVO_PERIOD);
}
} }
int StepGen3::rtapi_app_main() int StepGen3::rtapi_app_main()
@@ -1136,7 +1144,6 @@ int StepGen3::rtapi_app_main()
printf("STEPGEN: ERROR: no channels configured\n"); printf("STEPGEN: ERROR: no channels configured\n");
return -1; return -1;
} }
printf("%d channels configured step_type=%d ctrl_type=%d\n", num_chan, step_type[0], parse_ctrl_type(ctrl_type[0]));
/* periodns will be set to the proper value when 'make_pulses()' runs for /* periodns will be set to the proper value when 'make_pulses()' runs for
the first time. We load a default value here to avoid glitches at the first time. We load a default value here to avoid glitches at
startup, but all these 'constants' are recomputed inside startup, but all these 'constants' are recomputed inside

88
Firmware/src/main.cpp
View File

@@ -9,11 +9,11 @@ _Objects Obj;
HardwareSerial Serial1(PA10, PA9); HardwareSerial Serial1(PA10, PA9);
// STG = StepGenTest // SGT = StepGenTest
#define SGT 0 #define SGT 1
#if SGT #if SGT
volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
#define DEBUG_TIM8 1
#include "MyEncoder.h" #include "MyEncoder.h"
void indexPulseEncoderCB1(void); void indexPulseEncoderCB1(void);
MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1); MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1);
@@ -22,7 +22,10 @@ void indexPulseEncoderCB1(void)
Encoder1.indexPulse(); Encoder1.indexPulse();
} }
#endif #endif
#include "StepGen3.h" #include "StepGen3.h"
StepGen3 *Step = 0;
#if SGT #if SGT
#include "extend32to64.h" #include "extend32to64.h"
@@ -32,8 +35,8 @@ extend32to64 longTime;
void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
{ {
Encoder1.setLatch(Obj.IndexLatchEnable); // Encoder1.setLatch(Obj.IndexLatchEnable);
Encoder1.setScale(2000); // Encoder1.setScale(2000);
// Step2.reqPos(Obj.CommandedPosition2); // Step2.reqPos(Obj.CommandedPosition2);
// Step2.setScale(Obj.StepsPerMM2); // Step2.setScale(Obj.StepsPerMM2);
@@ -45,18 +48,27 @@ uint64_t reallyNowTime = 0, reallyThenTime = 0;
uint64_t timeDiff; // Timediff in nanoseconds uint64_t timeDiff; // Timediff in nanoseconds
void handleStepper(void) void handleStepper(void)
{ {
double pos[MAX_CHAN];
pos[0] = Obj.CommandedPosition1;
pos[1] = Obj.CommandedPosition2;
if (Step)
{
Step->updateStepGen(pos);
Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
Obj.ActualPosition1 = Step->stepgen_array[1].pos_fb;
}
} }
void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
{ {
Obj.IndexStatus = Encoder1.indexHappened(); // Obj.IndexStatus = Encoder1.indexHappened();
Obj.EncPos = Encoder1.currentPos(); // Obj.EncPos = Encoder1.currentPos();
Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); // Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
Obj.IndexByte = Encoder1.getIndexState(); // Obj.IndexByte = Encoder1.getIndexState();
float_t ap2 = Obj.ActualPosition2; float_t ap2 = Obj.ActualPosition2;
Obj.ActualPosition1 = Obj.CommandedPosition1; // Step1.actPos(); Obj.ActualPosition1 = Obj.CommandedPosition1; // Step1.actPos();
Obj.ActualPosition2 = Obj.CommandedPosition2; // Step2.actPos(); Obj.ActualPosition2 = Obj.CommandedPosition2; // Step2.actPos();
#if 0
uint64_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds uint64_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
Tim.push(dTim); Tim.push(dTim);
uint64_t max_Tim = 0, min_Tim = UINT64_MAX; uint64_t max_Tim = 0, min_Tim = UINT64_MAX;
@@ -69,6 +81,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
min_Tim = aTim; min_Tim = aTim;
} }
thenTime = irqTime; thenTime = irqTime;
#endif
Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
Obj.D1 = 0; Obj.D1 = 0;
Obj.D2 = 0; Obj.D2 = 0;
@@ -104,46 +117,54 @@ static esc_cfg_t config =
volatile byte serveIRQ = 0; volatile byte serveIRQ = 0;
#endif #endif
StepGen3 *Step = 0;
void basePeriodCB(void) void basePeriodCB(void)
{ {
if (Step && Step->stepgen_array) if (Step && Step->stepgen_array)
{ {
Step->make_pulses(Step->stepgen_array, BASE_PERIOD); Step->make_pulses(Step->stepgen_array, BASE_PERIOD);
stepgen_t *step; stepgen_t *step;
step = &(Step->stepgen_array[0]); for (int i = 0; i < Step->num_chan; i++)
digitalWrite(PA6, step->phase[DIR_PIN] ? LOW : HIGH); {
digitalWrite(PA7, step->phase[STEP_PIN] ? LOW : HIGH); step = &(Step->stepgen_array[i]);
digitalWrite(Step->dirPin[i], step->phase[DIR_PIN] ? LOW : HIGH);
digitalWrite(Step->stepPin[i], step->phase[STEP_PIN] ? LOW : HIGH);
}
} }
} }
void setup(void) void setup(void)
{ {
Serial1.begin(115200);
#if SGT
rcc_config(); // probably breaks some timers.
ecat_slv_init(&config);
#endif
pinMode(PA6, OUTPUT);
pinMode(PA7, OUTPUT);
digitalWrite(PA6, HIGH);
digitalWrite(PA7, HIGH);
Step = new StepGen3;
HardwareTimer *MyTim = new HardwareTimer(TIM3); Serial1.begin(115200);
MyTim->setOverflow(BASE_PERIOD, MICROSEC_FORMAT);
rcc_config(); // Needed by encoder, probably breaks some timers.
ecat_slv_init(&config);
pinMode(PA11, OUTPUT);
pinMode(PA12, OUTPUT);
digitalWrite(PA11, HIGH);
digitalWrite(PA12, HIGH);
pinMode(PC9, OUTPUT);
pinMode(PC10, OUTPUT);
digitalWrite(PC9, HIGH);
digitalWrite(PC10, HIGH);
// Step = new StepGen3;
HardwareTimer *MyTim = new HardwareTimer(TIM1); // The base period timer
MyTim->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT);
MyTim->attachInterrupt(basePeriodCB); MyTim->attachInterrupt(basePeriodCB);
MyTim->resume(); // MyTim->resume();
} }
double pos = 0; double pos = 0;
void loop(void) void loop(void)
{ {
Step->test(pos); pinMode(PC9, OUTPUT);
pos += 1; digitalWrite(PC9, LOW);
delay(1000); delay(50);
#if SGT digitalWrite(PC9, HIGH);
delay(50);
uint64_t dTime; uint64_t dTime;
if (serveIRQ) if (serveIRQ)
{ {
@@ -158,9 +179,8 @@ void loop(void)
ecat_slv_poll(); ecat_slv_poll();
} }
dTime = longTime.extendTime(micros()) - irqTime; dTime = longTime.extendTime(micros()) - irqTime;
if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt // if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt
ecat_slv_poll(); ecat_slv_poll();
#endif
} }
void sync0Handler(void) void sync0Handler(void)