From 369a795ce5a5dfa61699309d683e707eb70195a0 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Sat, 23 Mar 2024 18:02:38 +0100 Subject: [PATCH] wip --- Firmware/include/StepGen3.h | 10 +++-- Firmware/src/StepGen3.cpp | 41 ++++++++++------- Firmware/src/main.cpp | 90 ++++++++++++++++++++++--------------- 3 files changed, 86 insertions(+), 55 deletions(-) diff --git a/Firmware/include/StepGen3.h b/Firmware/include/StepGen3.h index 12f555d..3aafed0 100755 --- a/Firmware/include/StepGen3.h +++ b/Firmware/include/StepGen3.h @@ -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 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 + uint32_t stepPin[MAX_CHAN] = {0}; + uint32_t dirPin[MAX_CHAN] = {0}; stepgen_t *stepgen_array = 0; @@ -88,7 +90,7 @@ public: volatile double recip_dt; // recprocal of period, avoids divides */ StepGen3(void); - void test(double pos_cmd); + void updateStepGen(double *pos_cmd); int rtapi_app_main(); int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode); void make_pulses(void *arg, long period); @@ -143,7 +145,9 @@ private: // For the example #define BASE_PERIOD 50000 #define SERVO_PERIOD 1000000 -#define JOINT_0_STEPGEN_MAXACCEL 520.0 -#define JOINT_0_SCALE 200 +#define JOINT_X_STEPGEN_MAXACCEL 520.0 +#define JOINT_X_SCALE 200 +#define JOINT_Z_STEPGEN_MAXACCEL 520.0 +#define JOINT_Z_SCALE 80 #endif diff --git a/Firmware/src/StepGen3.cpp b/Firmware/src/StepGen3.cpp index df6491e..42353f2 100755 --- a/Firmware/src/StepGen3.cpp +++ b/Firmware/src/StepGen3.cpp @@ -955,8 +955,7 @@ int StepGen3::export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mo } printf("Exporting %d\n", num); - // *(addr->phase[STEP_PIN]) = 0; // Several variants possible - // *(addr->phase[DIR_PIN]) = 0; + /* set default parameter values */ addr->pos_scale = 1.0; addr->old_scale = 0.0; @@ -1086,24 +1085,33 @@ StepGen3::StepGen3(void) step_type[0] = 0; ctrl_type[0] = (char *)malloc(2); 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"); - (void)rtapi_app_main(); - printf("Hello World2\n"); - stepgen_t *step; - step = &(stepgen_array[0]); - step->enable = 1; - step->pos_scale = JOINT_0_SCALE; - step->maxaccel = JOINT_0_STEPGEN_MAXACCEL; + rtapi_app_main(); + stepgen_array[0].enable = 1; + stepgen_array[0].pos_scale = JOINT_X_SCALE; + stepgen_array[0].maxaccel = JOINT_X_STEPGEN_MAXACCEL; + stepgen_array[1].pos_scale = JOINT_Z_SCALE; + stepgen_array[1].maxaccel = JOINT_Z_STEPGEN_MAXACCEL; + stepgen_array[0].enable = stepgen_array[1].enable = 1; } -void StepGen3::test(double pos_cmd) +void StepGen3::updateStepGen(double *pos_cmd) { - stepgen_t *step; - step = &(stepgen_array[0]); - step->pos_cmd = pos_cmd; - update_pos(step, SERVO_PERIOD); - update_freq(step, SERVO_PERIOD); + for (int i = 0; i < num_chan; i++) + { + stepgen_t *step; + step = &(stepgen_array[i]); + step->pos_cmd = pos_cmd[i]; + update_pos(step, SERVO_PERIOD); + update_freq(step, SERVO_PERIOD); + } } int StepGen3::rtapi_app_main() @@ -1136,7 +1144,6 @@ int StepGen3::rtapi_app_main() printf("STEPGEN: ERROR: no channels configured\n"); 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 the first time. We load a default value here to avoid glitches at startup, but all these 'constants' are recomputed inside diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 33ee6db..6810a05 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -9,11 +9,11 @@ _Objects Obj; HardwareSerial Serial1(PA10, PA9); -// STG = StepGenTest -#define SGT 0 +// SGT = StepGenTest +#define SGT 1 #if SGT volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt -#define DEBUG_TIM8 1 + #include "MyEncoder.h" void indexPulseEncoderCB1(void); MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1); @@ -22,7 +22,10 @@ void indexPulseEncoderCB1(void) Encoder1.indexPulse(); } #endif + #include "StepGen3.h" +StepGen3 *Step = 0; + #if SGT #include "extend32to64.h" @@ -32,8 +35,8 @@ extend32to64 longTime; void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation { - Encoder1.setLatch(Obj.IndexLatchEnable); - Encoder1.setScale(2000); + // Encoder1.setLatch(Obj.IndexLatchEnable); + // Encoder1.setScale(2000); // Step2.reqPos(Obj.CommandedPosition2); // Step2.setScale(Obj.StepsPerMM2); @@ -45,18 +48,27 @@ uint64_t reallyNowTime = 0, reallyThenTime = 0; uint64_t timeDiff; // Timediff in nanoseconds 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 { - Obj.IndexStatus = Encoder1.indexHappened(); - Obj.EncPos = Encoder1.currentPos(); - Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); - Obj.IndexByte = Encoder1.getIndexState(); + // Obj.IndexStatus = Encoder1.indexHappened(); + // Obj.EncPos = Encoder1.currentPos(); + // Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); + // Obj.IndexByte = Encoder1.getIndexState(); float_t ap2 = Obj.ActualPosition2; Obj.ActualPosition1 = Obj.CommandedPosition1; // Step1.actPos(); Obj.ActualPosition2 = Obj.CommandedPosition2; // Step2.actPos(); - +#if 0 uint64_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds Tim.push(dTim); 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; } thenTime = irqTime; +#endif Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug Obj.D1 = 0; Obj.D2 = 0; @@ -104,46 +117,54 @@ static esc_cfg_t config = volatile byte serveIRQ = 0; #endif -StepGen3 *Step = 0; - void basePeriodCB(void) { if (Step && Step->stepgen_array) { Step->make_pulses(Step->stepgen_array, BASE_PERIOD); stepgen_t *step; - step = &(Step->stepgen_array[0]); - digitalWrite(PA6, step->phase[DIR_PIN] ? LOW : HIGH); - digitalWrite(PA7, step->phase[STEP_PIN] ? LOW : HIGH); + for (int i = 0; i < Step->num_chan; i++) + { + 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) { - 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); - MyTim->setOverflow(BASE_PERIOD, MICROSEC_FORMAT); + Serial1.begin(115200); + + 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->resume(); + // MyTim->resume(); } double pos = 0; void loop(void) { - Step->test(pos); - pos += 1; - delay(1000); -#if SGT + pinMode(PC9, OUTPUT); + digitalWrite(PC9, LOW); + delay(50); + digitalWrite(PC9, HIGH); + delay(50); + uint64_t dTime; if (serveIRQ) { @@ -158,9 +179,8 @@ void loop(void) ecat_slv_poll(); } dTime = longTime.extendTime(micros()) - irqTime; - if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt - ecat_slv_poll(); -#endif + // if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt + ecat_slv_poll(); } void sync0Handler(void)