From 222e5857dc9ee62fa993bc51bb5643deeea65f8e Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Sun, 7 Jan 2024 19:38:22 +0100 Subject: [PATCH] Reorganize --- Firmware/src/main.cpp | 99 ++++++++++++++++++++++--------------------- 1 file changed, 51 insertions(+), 48 deletions(-) diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index e2edede..26b11e3 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -27,7 +27,6 @@ double PosScaleRes = 1.0; uint32_t CurPosScale = 1; uint8_t OldLatchCEnable = 0; volatile uint8_t indexPulseFired = 0; -uint32_t nFires = 0; volatile uint8_t pleaseZeroTheCounter = 0; #define STEPPER_DIR_PIN PA12 @@ -66,7 +65,6 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation { Obj.IndexStatus = 1; indexPulseFired = 0; - nFires++; PreviousEncoderCounterValue = 0; } // Obj.DiffT = sync0CycleTime; @@ -97,6 +95,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation void ESC_interrupt_enable(uint32_t mask); void ESC_interrupt_disable(uint32_t mask); uint16_t dc_checker(void); +void TimerStep_CB(void); +void sync0Handler(void); +void handleStepper(void); +void makePulses(uint32_t period /* in usecs */, int32_t pulses /* nr of pulses to do*/); static esc_cfg_t config = { @@ -106,7 +108,7 @@ static esc_cfg_t config = .set_defaults_hook = NULL, .pre_state_change_hook = NULL, .post_state_change_hook = NULL, - .application_hook = handleStepper, // StepGen, + .application_hook = handleStepper, .safeoutput_override = NULL, .pre_object_download_hook = NULL, .post_object_download_hook = NULL, @@ -118,28 +120,9 @@ static esc_cfg_t config = .esc_check_dc_handler = dc_checker, }; -void TimerStep_CB(void) -{ - stepCount++; - if (stepCount == stepPulses) - { - MyTim->pause(); - } -} - -void makePulses(uint32_t period /* in usecs */, int32_t pulses /* nr of pulses to do*/) -{ - byte sgn = pulses > 0 ? HIGH : LOW; - digitalWrite(STEPPER_DIR_PIN, sgn); // I think one should really wait a bit when changed - uint32_t puls = abs(pulses); - MyTim->setOverflow(abs(pulses) * 1000000 / period, HERTZ_FORMAT); - MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % - stepCount = 0; - stepPulses = abs(pulses); - MyTim->resume(); -} - void sync0Handler(void); +volatile byte serveIRQ = 0; + void setup(void) { Serial1.begin(115200); @@ -153,14 +136,14 @@ void setup(void) // Set starting count value EncoderInit.SetCount(Tim2, 0); - attachInterrupt(digitalPinToInterrupt(INDEX_PIN), indexPulse, RISING); // Always when Index triggered + attachInterrupt(digitalPinToInterrupt(INDEX_PIN), indexPulse, RISING); // When Index triggered // EncoderInit.SetCount(Tim3, 0); // EncoderInit.SetCount(Tim4, 0); // EncoderInit.SetCount(Tim8, 0); ecat_slv_init(&config); } -volatile byte serveIRQ = 0; + void loop(void) { ESCvar.PrevTime = ESCvar.Time; @@ -173,25 +156,6 @@ void loop(void) ecat_slv_poll(); } -#define ONE_PERIOD 65536 -#define HALF_PERIOD 32768 - -int64_t unwrap_encoder(uint16_t in, int64_t *prev) -{ - int64_t c64 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap - int64_t dif = (c64 - *prev); // core concept: prev + (current - prev) = current - - // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result - int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD; - if (dif < -HALF_PERIOD) - mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C - - int64_t unwrapped = *prev + mod_dif; - *prev = unwrapped; // load previous value - - return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return -} - void indexPulse(void) { if (pleaseZeroTheCounter) @@ -211,13 +175,34 @@ void sync0Handler(void) void handleStepper(void) { - byte forwardDirection = 0; // 1 if going forward - int32_t pulsesToGo = 100 * (requestedPosition - actualPosition); + int32_t pulsesToGo = 5000 * (requestedPosition - actualPosition); // From Turner.hal X:5000 Z:2000 ps/mm if (pulsesToGo != 0) - makePulses(900, pulsesToGo); // Make the pulses using hardware timer + makePulses(sync0CycleTime, pulsesToGo); // Make the pulses using hardware timer actualPosition = requestedPosition; } + +void makePulses(uint32_t period /* in usecs */, int32_t pulses /* nr of pulses to do*/) +{ + byte sgn = pulses > 0 ? HIGH : LOW; + digitalWrite(STEPPER_DIR_PIN, sgn); // I think one should really wait a bit when changed + uint32_t puls = abs(pulses); + MyTim->setOverflow(abs(pulses) * 1000000 / period, HERTZ_FORMAT); + MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % + stepCount = 0; + stepPulses = abs(pulses); + MyTim->resume(); +} + +void TimerStep_CB(void) +{ + stepCount++; + if (stepCount == stepPulses) + { + MyTim->pause(); + } +} + void ESC_interrupt_enable(uint32_t mask) { // Enable interrupt for SYNC0 or SM2 or SM3 @@ -266,3 +251,21 @@ uint16_t dc_checker(void) sync0CycleTime = ESC_SYNC0cycletime(); return 0; } +#define ONE_PERIOD 65536 +#define HALF_PERIOD 32768 + +int64_t unwrap_encoder(uint16_t in, int64_t *prev) +{ + int64_t c64 = (int32_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap + int64_t dif = (c64 - *prev); // core concept: prev + (current - prev) = current + + // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result + int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD; + if (dif < -HALF_PERIOD) + mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C + + int64_t unwrapped = *prev + mod_dif; + *prev = unwrapped; // load previous value + + return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return +} \ No newline at end of file