Dual Brushless Motor ESC, 10-62V, up to 50A per motor. Motors ganged or independent, multiple control input methods, cycle-by-cycle current limit, speed mode and torque mode control. Motors tiny to kW. Speed limit and other parameters easily set in firmware. As used in 'The Brushless Brutalist' locomotive - www.jons-workshop.com. See also Model Engineer magazine June-October 2019.
Dependencies: mbed BufferedSerial Servo PCT2075 FastPWM
Update 17th August 2020 Radio control inputs completed
Diff: main.cpp
- Revision:
- 16:d1e4b9ad3b8b
- Parent:
- 15:2591e2008323
- Child:
- 17:cc9b854295d6
--- a/main.cpp Sat Nov 30 18:40:30 2019 +0000 +++ b/main.cpp Tue Jun 09 09:20:19 2020 +0000 @@ -1,7 +1,7 @@ /* STM3_ESC Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control. Jon Freeman B. Eng Hons - 2015 - 2019 + 2015 - 2020 */ #include "mbed.h" #include "STM3_ESC.h" @@ -10,11 +10,13 @@ #include "Servo.h" #include "brushless_motor.h" #include "Radio_Control_In.h" -//#ifdef TARGET_NUCLEO_F401RE // - +#include "LM75B.h" // New I2C temp sensor code March 2020 (to suit possible next board issue, harmless otherwise) +//#ifdef TARGET_NUCLEO_F401RE // Target is TARGET_NUCLEO_F401RE for all boards produced. //#endif /* Brushless_STM3_ESC board +Apr 2020 * RC tested on 'Idle Halt' branch line - all good - also tested Inverter Gen power sorce. All good. +Dec 2019 * Radio control inputs now fully implemented, requires fitting tiny 'RC_in_fix' board. Jan 2019 * WatchDog implemented. Default is disabled, 'kd' command from TS controller enables and reloads * Tidied brushless_motor class, parameter passing now done properly * class RControl_In created. Inputs now routed to new pins, can now use two chans both class RControl_In and Servo out @@ -29,6 +31,7 @@ LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable March 2019 temp sensor only included with TEMP_SENSOR_ENABLE defined. Temp reading not essential, LMT01 was not a good choice due to significant loading of interrupts, threatening integrity of Real Time System + *** New sensor code for LM75B temp sensor added March 2020 *** */ @@ -39,7 +42,7 @@ ____________________________________________________________________________________ CONTROL PHILOSOPHY -This STM3_ESC Bogie drive board software should ensure sensible control when commands supplied are not sensible ! +This STM3_ESC Dual Brushless Motor drive board software should ensure sensible control when commands supplied are not sensible ! That is, smooth transition between Drive, Coast and Brake to be implemented here. The remote controller must not be relied upon to deliver sensible command sequences. @@ -51,63 +54,58 @@ */ -#if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP +//#if defined (TARGET_NUCLEO_L476RG) // CPU in 64 pin LQFP ** NOT PROVED ** No good, pinmap different +//#include "F401RE.h" +//#endif +#if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP - This is what all produced boards have #include "F401RE.h" #endif -#if defined (TARGET_NUCLEO_F411RE) // CPU in 64 pin LQFP +#if defined (TARGET_NUCLEO_F411RE) // CPU in 64 pin LQFP - Never tried, but probably would work as is #include "F411RE.h" #endif #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP #include "F446ZE.h" // A thought for future version #endif /* Global variable declarations */ -char const_version_string[] = {"1.0.y2019.m09.d29\0"}; // Version string, readable from serial ports -volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US -//int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup -int WatchDog = 0; // Set this up in main once pre-flight checks done. Allow extra few seconds at powerup -bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable -uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts + +//volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US +uint32_t WatchDog = 0, // Set this up in main once pre-flight checks done. Allow extra few seconds at powerup + volt_reading = 0, // Global updated by interrupt driven read of Battery Volts driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US, 31250us at Sept 2019 AtoD_Semaphore = 0; +bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable bool loop_flag = false; // made true in ISR_loop_timer, picked up and made false again in main programme loop bool flag_8Hz = false; // As loop_flag but repeats 8 times per sec -bool temp_sensor_exists = false; -double rpm2mph; +bool temp_sensor_exists = false; // March 2020 - Now used to indicate presence or not of LM75B i2c temp sensor -double Current_Scaler_Sep_2019 = 1.0; // New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom. - // See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I - // at lower voltages. This is simply because current takes longer to build in motor inductance when voltage - // is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning - // similar current flows for shorter times at higher voltages. - +/* + * Not used since LMT01 proved not a good choice. See LM75B code added March 2020 + * #ifdef TEMP_SENSOR_ENABLE uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01 last_temperature_count = 0; // global updated approx every 100ms after each LMT01 conversion completes -#endif -/* End of Global variable declarations */ - -Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc -Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop -#ifdef TEMP_SENSOR_ENABLE Ticker temperature_find_ticker; Timer temperature_timer; #endif -#ifdef USING_DC_MOTORS -Timer dc_motor_kicker_timer; -Timeout motors_restore; -#endif +*/ +/* End of Global variable declarations */ -RControl_In RC_chan_1 (PC_14); -RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin +Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc - 50 us +Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop + +eeprom_settings user_settings (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup +RControl_In RC_chan_1 (PC_14); +RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes. -eeprom_settings mode (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup +//PCT2075 temp_sensor( i2c ); // or LM75B temp_sensor( p?, p? ); Added March 2020 +PCT2075 temp_sensor( SDA_PIN, SCL_PIN ); // or LM75B temp_sensor( p?, p? ); Added March 2020 -//uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0; + /* - 5 1 3 2 6 4 SENSOR SEQUENCE + 5 1 3 2 6 4 Brushless Motor Hall SENSOR SEQUENCE 1 1 1 1 0 0 0 ---___---___ Hall1 2 0 0 1 1 1 0 __---___---_ Hall2 @@ -115,36 +113,39 @@ UV WV WU VU VW UW OUTPUT SEQUENCE -8th July 2018 -Added drive to DC brushed motors. -Connect U and W to dc motor, leave V open. - -Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. Use this for dc motors +Dec 2019 Support for DC motors deleted. +Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. */ -const uint16_t A_tabl[] = { // Origial table - 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake - 0, AWHVL,AVHUL,AWHUL,AUHWL,AUHVL,AVHWL,AUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 - 0, AVHWL,AUHVL,AUHWL,AWHUL,AVHUL,AWHVL,AWHUL, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1 - 0, BRA,BRA,BRA,BRA,BRA,BRA,BRA, // Regenerative Braking +const uint16_t A_tabl[] = { // Table of motor energisation bit patterns. Rows are Handbrake, Forward, Reverse, Regen brake. Cols relate to Hall sensor outputs +// AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH + 0, AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH, 0, // Handbrake +// 1->3 2->6 3->2 4->5 5->1 6->4 + 0, AWHVL, AVHUL, AWHUL, AUHWL, AUHVL, AVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, +// 1->5 2->3 3->1 4->6 5->4 6->2 + 0, AVHWL, AUHVL, AUHWL, AWHUL, AVHUL, AWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, + 0, BRA, BRA, BRA, BRA, BRA, BRA, BRA, // Regenerative Braking KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33] } ; - -const uint16_t B_tabl[] = { - 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake - 0, BWHVL,BVHUL,BWHUL,BUHWL,BUHVL,BVHWL,BUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 - 0, BVHWL,BUHVL,BUHWL,BWHUL,BVHUL,BWHVL,BWHUL, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1 - 0, BRB,BRB,BRB,BRB,BRB,BRB,BRB, // Regenerative Braking +// AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH +const uint16_t B_tabl[] = { // Different tables for Motors A and B as different ports and different port bits used. +// 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake + 0, BUHVL|BWL, BWHUL|BVL, BWHVL|BUH, BVHWL|BUL, BUHWL|BVH, BVHUL|BWH, 0, // Handbrake + 0, BWHVL, BVHUL, BWHUL, BUHWL, BUHVL, BVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, + 0, BVHWL, BUHVL, BUHWL, BWHUL, BVHUL, BWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, + 0, BRB, BRB, BRB, BRB, BRB, BRB, BRB, // Regenerative Braking KEEP_L_MASK_B, KEEP_H_MASK_B } ; brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK, ISHUNTA); -brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB); - +brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB); // Two brushless motor instantiations extern cli_2019 pcc, tsc; // command line interpreters from pc and touch screen +static const int LM75_I2C_ADDR = 0x90; // i2c temperature sensor + // Interrupt Service Routines +/* #ifdef TEMP_SENSOR_ENABLE void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec { @@ -158,13 +159,25 @@ if (t == 6) readflag = false; } + +void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some +{ + int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ? + temperature_timer.reset (); + temp_sensor_count++; + if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading + temp_sensor_count++; +// T2 = !T2; // scope hanger +} #endif +*/ + /** void ISR_loop_timer () * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above) * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop. * Increments global 'sys_timer', usable anywhere as general measure of elapsed time */ -void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US +void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US (32Hz) { loop_flag = true; // set flag to allow main programme loop to proceed sys_timer++; // Just a handy measure of elapsed time for anything to use @@ -180,31 +193,41 @@ void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50 { AtoD_Semaphore++; - fast_sys_timer++; // Just a handy measure of elapsed time for anything to use +// fast_sys_timer++; // Just a handy measure of elapsed time for anything to use +} + +// End of Interrupt Service Routines + +const char * get_version () { + return "1.0.y2020.m05.d21\0"; // Version string, readable using 'ver' serial command } -#ifdef TEMP_SENSOR_ENABLE -void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some -{ - int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ? - temperature_timer.reset (); - temp_sensor_count++; - if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading - temp_sensor_count++; -// T2 = !T2; // scope hanger +bool read_temperature (float & t) { +// pc.printf ("test param temp = %7.3f\r\n", t); + if (!temp_sensor_exists) + return false; + t = temp_sensor; + return true; } -#endif -// End of Interrupt Service Routines - -void setVI (double v, double i) +void setVI_A (double v, double i) { MotorA.set_V_limit (v); // Sets max motor voltage MotorA.set_I_limit (i); // Sets max motor current +} + +void setVI_B (double v, double i) +{ MotorB.set_V_limit (v); MotorB.set_I_limit (i); } +void setVI_both (double v, double i) +{ + setVI_A (v, i); + setVI_B (v, i); +} + /** * void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr @@ -217,11 +240,11 @@ MotorA.high_side_off (); if (MotorB.tickleon) MotorB.high_side_off (); - if (AtoD_Semaphore > 20) { + if (AtoD_Semaphore > 10) { pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore); - AtoD_Semaphore = 20; + AtoD_Semaphore = 10; } - while (AtoD_Semaphore > 0) { + while (AtoD_Semaphore > 0) { // semaphore gets incremented in timer interrupt handler, t=50us AtoD_Semaphore--; // Code here to sequence through reading voltmeter, demand pot, ammeters switch (i) { // @@ -254,36 +277,6 @@ } } -/** double Read_Servo1_In () -* driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine -* ISR also filters signal -* This function returns normalised double (range 0.0 to 1.0) representation of driver pot position -*/ -double Read_Servo1_In () -{ - const double xjoin = 0.5, - yjoin = 0.35, - slope_a = yjoin / xjoin, - slope_b = (1.0 - yjoin)/(1.0 - xjoin); -// centre = 0.35, // For pot, first (1/3)ish in braking area, position = 1/3 drift, > (1/3)ish drive -// halfish = 0.5; // RC stick in the centre value - // Bottom half of RC stick movement maps to lowest (1/3)ish pot movement - // Higher half of RC stick movement maps to highest (2/3)ish pot movement - double t; - double demand = RC_chan_1.normalised(); - // apply demand = 1.0 - demand here to swap stick move polarity -// return pow (demand, SERVOIN_PWR_BENDER); - if (demand < 0.0) demand = 0.0; - if (demand > 1.0) demand = 1.0; - if (demand < xjoin) { - demand *= slope_a; - } - else { - t = yjoin + ((demand - xjoin) * slope_b); - demand = t; - } - return demand; -} /** double Read_DriverPot () * driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine @@ -301,71 +294,26 @@ } -void Update_Current_Scaler () { // ***NEW Sept 2019*** Called at 8Hz -const double Vnom = 48.0, - Vmin = Vnom / 3.0, - Voff = Vnom / 4.0; - - double v = Read_BatteryVolts (); - if (v > Vnom) - v = Vnom; - if (v < Voff) - v = Voff; - if (v > Vmin) { // In expected normal operating voltage range - Current_Scaler_Sep_2019 = v / Vnom; // May need to revisit law - } - else { // In very low voltage region - Current_Scaler_Sep_2019 = 0.5 * (v / Vnom); - } +double trim (const double min, const double max, double input) { + if (input < min) input = min; + if (input > max) input = max; + return input; } -void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb +void brake_motors_both (double brake_effort) { + MotorA.brake (brake_effort); + MotorB.brake (brake_effort); +} + + +void mode_set_motors_both (int mode) // called from cli to set fw, re, rb, hb { MotorA.set_mode (mode); MotorB.set_mode (mode); - if (mode == MOTOR_REGENBRAKE) { - if (val > 1.0) - val = 1.0; - if (val < 0.0) - val = 0.0; - val *= 0.9; // set upper limit, this is essential - val = sqrt (val); // to linearise effect - setVI (val, 1.0); - } } - -#ifdef USING_DC_MOTORS -void restorer () -{ - motors_restore.detach (); - if (MotorA.dc_motor) { - MotorA.set_V_limit (MotorA.last_V); - MotorA.set_I_limit (MotorA.last_I); - MotorA.motor_set (); - } - if (MotorB.dc_motor) { - MotorB.set_V_limit (MotorB.last_V); - MotorB.set_I_limit (MotorB.last_I); - MotorB.motor_set (); - } -} -#endif - -void rcin_report () { - double c1 = RC_chan_1.normalised(); - double c2 = RC_chan_2.normalised(); - uint32_t pc1 = RC_chan_1.pulsecount(); - uint32_t pc2 = RC_chan_2.pulsecount(); - pc.printf ("At rcin_report, Ch1=%.3f, Ch2=%.3f, cnt1 %d, cnt2 %d\r\n", c1, c2, pc1, pc2); -// if (c1 < -0.0001) - pc.printf ("\t1 period %d, pulsewidth %d\r\n", RC_chan_1.period(), RC_chan_1.pulsewidth()); -// if (c2 < -0.0001) - pc.printf ("\t2 period %d, pulsewidth %d\r\n", RC_chan_2.period(), RC_chan_2.pulsewidth()); -} - -bool worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes +bool is_it_worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes double c = a - b; if (c < 0.0) c = 0.0 - c; @@ -374,9 +322,9 @@ return false; } -void hand_control_state_machine (int source) { // if hand control mode '3', get here @ approx 30Hz to read drivers control pot - // if servo1 mode '4', reads input from servo1 instead -enum { // states used in hand_control_state_machine() +void hand_control_state_machine (int source) { // if hand control user_settings '3', get here @ approx 30Hz to read drivers control pot + // if servo1 user_settings '4', reads input from servo1 instead +enum { // states used in RC_or_hand_control_state_machine() HAND_CONT_IDLE, HAND_CONT_BRAKE_WAIT, HAND_CONT_BRAKE_POT, @@ -392,7 +340,7 @@ static int dirbuf[5] = {100,100,100,100,100}; // Initialised with invalid direction such that 'change' is read from switch static int dirbufptr = 0, direction_old = -1, direction_new = -1; const int buflen = sizeof(dirbuf) / sizeof(int); - const double Pot_Brake_Range = 0.35; //pow (0.5, SERVOIN_PWR_BENDER); //0.353553 for SERVOIN_PWR_BENDER = 1.5; + double User_Brake_Range; // direction_old = direction_new; @@ -410,10 +358,9 @@ hand_controller_state = HAND_CONT_BRAKE_WAIT; // validated change of direction switch switch (source) { - case 3: // driver pot is control input + case HAND: // driver pot is control input pot_position = Read_DriverPot (); // Only place read in the loop, rteads normalised 0.0 to 1.0 - break; - case 4: // servo 1 input is control input + User_Brake_Range = user_settings.user_brake_range(); break; default: pot_position = 0.0; @@ -427,14 +374,14 @@ case HAND_CONT_BRAKE_WAIT: // Only get here after direction input changed or newly validated at power on pc.printf ("At HAND_CONT_BRAKE_WAIT\r\n"); brake_effort = 0.05; // Apply braking not too fiercely - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change + brake_motors_both (brake_effort); hand_controller_state = HAND_CONT_BRAKE_POT; break; case HAND_CONT_BRAKE_POT: // Only get here after one pass through HAND_CONT_BRAKE_WAIT but - if (brake_effort < 0.9) { // remain in this state until driver has turned pott fully anticlockwise + if (brake_effort < 0.95) { // remain in this state until driver has turned pott fully anticlockwise brake_effort += 0.05; // ramp braking up to max over about one sec after direction change or validation - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change or testing at power on + brake_motors_both (brake_effort); // Direction change or testing at power on pc.printf ("Brake effort %.2f\r\n", brake_effort); } else { // once braking up to quite hard @@ -445,24 +392,23 @@ break; case HAND_CONT_STATE_INTO_BRAKING: - brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; + brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range; if (brake_effort < 0.0) brake_effort = 0.5; - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); + brake_motors_both (brake_effort); old_pot_position = pot_position; hand_controller_state = HAND_CONT_STATE_BRAKING; pc.printf ("Brake\r\n"); break; case HAND_CONT_STATE_BRAKING: - if (pot_position > Pot_Brake_Range) // escape braking into drive + if (pot_position > User_Brake_Range) // escape braking into drive hand_controller_state = HAND_CONT_STATE_INTO_DRIVING; else { - if (worth_the_bother(pot_position, old_pot_position)) { + if (is_it_worth_the_bother(pot_position, old_pot_position)) { old_pot_position = pot_position; - brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; - mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); -// pc.printf ("Brake %.2f\r\n", brake_effort); + brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range; + brake_motors_both (brake_effort); } } break; @@ -470,20 +416,20 @@ case HAND_CONT_STATE_INTO_DRIVING: // Only get here after HAND_CONT_STATE_BRAKING pc.printf ("Drive\r\n"); if (direction_new == 1) - mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors + mode_set_motors_both (MOTOR_FORWARD); // sets both motors else - mode_set_both_motors (MOTOR_REVERSE, 0.0); + mode_set_motors_both (MOTOR_REVERSE); hand_controller_state = HAND_CONT_STATE_DRIVING; break; case HAND_CONT_STATE_DRIVING: - if (pot_position < Pot_Brake_Range) // escape braking into drive + if (pot_position < User_Brake_Range) // escape braking into drive hand_controller_state = HAND_CONT_STATE_INTO_BRAKING; else { - if (worth_the_bother(pot_position, old_pot_position)) { + if (is_it_worth_the_bother(pot_position, old_pot_position)) { old_pot_position = pot_position; - drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range); - setVI (drive_effort, drive_effort); // Wind volts and amps up and down together ??? + drive_effort = (pot_position - User_Brake_Range) / (1.0 - User_Brake_Range); + setVI_both (drive_effort, drive_effort); // Wind volts and amps up and down together ??? pc.printf ("Drive %.2f\r\n", drive_effort); } } @@ -495,68 +441,81 @@ } // endof switch (hand_controller_state) { } -int main() +void set_RCIN_offsets () { + RC_chan_1.set_offset (user_settings.rd(RCI1_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK)); + RC_chan_2.set_offset (user_settings.rd(RCI2_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK)); + RC_chan_1.set_chanmode (user_settings.rd(RCIN1), user_settings.rd(RCIN1REVERSE)) ; + RC_chan_2.set_chanmode (user_settings.rd(RCIN2), user_settings.rd(RCIN2REVERSE)) ; +} + + +int main() // Programme entry point { - int eighth_sec_count = 0; + uint32_t eighth_sec_count = 0; // Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc - tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator + tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator - 50 us loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator -#ifdef TEMP_SENSOR_ENABLE - temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960); - Temperature_pin.fall (&temp_sensor_isr); - Temperature_pin.mode (PullUp); - temperature_timer.start (); -#endif + // Done setting up system interrupt timers - pc.baud (9600); // COM port to pc com3.baud (1200); // Once had an idea to use this for IR comms, never tried com2.baud (19200); // Opto isolated serial bus connecting 'n' STM3_ESC boards to 1 Brute Touch Screen controller +// pc.baud (9600); // COM port to pc + pc.baud (user_settings.baud()); // COM port to pc - rpm2mph = 60.0 // to Motor Revs per hour; - * ((double)mode.rd(MOTPIN) / (double)mode.rd(WHEELGEAR)) // Wheel revs per hour - * PI * ((double)mode.rd(WHEELDIA) / 1000.0) // metres per hour - * 39.37 / (1760.0 * 36.0); // miles per hour +// pc.printf ("Baud rate %d\r\n", user_settings.baud()); - MotorA.direction_set (mode.rd(MOTADIR)); // modes all setup in class eeprom_settings {} mode ; constructor - MotorB.direction_set (mode.rd(MOTBDIR)); - MotorA.poles (mode.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise - MotorB.poles (mode.rd(MOTBPOLES)); // Only two calls are here - MotorA.set_mode (MOTOR_REGENBRAKE); - MotorB.set_mode (MOTOR_REGENBRAKE); - setVI (0.9, 0.5); // Power up with moderate regen braking applied + MotorA.set_direction (user_settings.rd(MOTADIR)); // user_settingss all setup in class eeprom_settings {} user_settings ; constructor + MotorB.set_direction (user_settings.rd(MOTBDIR)); + MotorA.poles (user_settings.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise + MotorB.poles (user_settings.rd(MOTBPOLES)); // Only two calls are here +// MotorA.set_mode (MOTOR_REGENBRAKE); // Done in motor constructor +// MotorB.set_mode (MOTOR_REGENBRAKE); +// setVI_both (0.9, 0.5); // Power up with moderate regen braking applied + set_RCIN_offsets (); + + class RC_stick_info RCstick1, RCstick2; // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor T2 = 0; // T2, T3, T4 As yet unused pins -// T3 = 0; -// T4 = 0; -// T5 = 0; now input from fw/re on remote control box +// T3 = 0;// T4 = 0;// T5 = 0; now input from fw/re on remote control box T6 = 0; + pc.printf ("\r\n\nSTM3_ESC Starting Ver %s, Command Source setting = %d\r\n", get_version(), user_settings.rd(COMM_SRC)); + pc.printf ("Designed by Jon Freeman B. Eng. Hons - 2017-2020. e jon@jons-workshop.com\r\n"); + if (user_settings.do_we_have_i2c (0xa0)) + pc.printf ("Got EEPROM, i2c error count = %d\r\n", user_settings.errs()); + else + pc.printf ("No eeprom found\r\n"); + pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true - Ready to Roll !" : "false"); -#ifdef TEMP_SENSOR_ENABLE - if ((last_temperature_count > 160) && (last_temperature_count < 2400)) // in range -40 to +100 degree C +//bool eeprom_settings::do_we_have_i2c (uint32_t x) + pc.printf ("LM75 temp sensor "); + if (user_settings.do_we_have_i2c (LM75_I2C_ADDR)) { + pc.printf ("reports temperature %7.3f\r\n", (float)temp_sensor ); temp_sensor_exists = true; -#endif -#ifdef USING_DC_MOTORS - dc_motor_kicker_timer.start (); -#endif - int old_hand_controller_direction = T5; - if (mode.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401 + } + else + pc.printf ("Not Fitted\r\n"); + + uint32_t old_hand_controller_direction = T5; + if (user_settings.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401 pc.printf ("Pot control\r\n"); if (T5) - mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors + mode_set_motors_both (MOTOR_FORWARD); // sets both motors else - mode_set_both_motors (MOTOR_REVERSE, 0.0); + mode_set_motors_both (MOTOR_REVERSE); } - - pc.printf ("About to start %s!, mode_bytes[COMM_SRC]= %d\r\n", const_version_string, mode.rd(COMM_SRC)); - pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true" : "false"); // pc.printf ("SystemCoreClock=%d, MAX_PWM_TICKS=%d\r\n", SystemCoreClock, MAX_PWM_TICKS); -// pcc.test () ; -// tsc.test () ; WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup - + pcc.flush (); // Flush serial rx buffers + tsc.flush (); +// pc.printf ("sizeof int is %d\r\n", sizeof(int)); // sizeof int is 4 + double Current_Scaler; // New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom. + // See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I + // at lower voltages. This is simply because current takes longer to build in motor inductance when voltage + // is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning + // similar current flows for shorter times at higher voltages. while (1) { // Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true while (!loop_flag) { // Most of the time is spent in this loop, repeatedly re-checking for commands from pc port @@ -566,62 +525,57 @@ } // 32Hz original setting for loop repeat rate loop_flag = false; // Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms - switch (mode.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever - case 0: // Invalid - break; - case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter - break; - case COM2: // COM2 - Primarily for testing, bypassed by command line interpreter + // double trim (const double min, const double max, double input) { + Current_Scaler = trim (0.1, 1.0, Read_BatteryVolts() / user_settings.Vnom()); + MotorA.I_scale (Current_Scaler); // Reduces motor current limits when voltage below nominal. + MotorB.I_scale (Current_Scaler); // This goes some way towards preventing engine stalls perhaps + + MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second + MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM + + RC_chan_1.read(RCstick1); // Get latest info from Radio Control inputs + RC_chan_2.read(RCstick2); + +//#define SERVO_OUT_TEST +#ifdef SERVO_OUT_TEST + if (RCstick1.active) Servo1 = RCstick1.deflection; + if (RCstick2.active) Servo2 = RCstick2.deflection; +#endif + + switch (user_settings.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever +// case 0: // Invalid +// break; +// case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter +// break; + case COM2: // COM2 - Nothing done here, all serial instructions handled in command line interpreter break; case HAND: // Put all hand controller input stuff here - hand_control_state_machine (3); + hand_control_state_machine (HAND); break; // endof hand controller stuff case RC_IN1: // RC_chan_1 - hand_control_state_machine (4); + RC_chan_1.energise (RCstick1, MotorA) ; // RC chan 1 drives both motors + RC_chan_1.energise (RCstick1, MotorB) ; break; case RC_IN2: // RC_chan_2 + RC_chan_2.energise (RCstick2, MotorA) ; // RC chan 2 drives both motors + RC_chan_2.energise (RCstick2, MotorB) ; + break; + case RC_IN_BOTH: // Robot Mode + RC_chan_1.energise (RCstick1, MotorA) ; // Two RC chans drive two motors independently + RC_chan_2.energise (RCstick2, MotorB) ; break; default: if (ESC_Error.read(FAULT_UNRECOGNISED_STATE)) { - pc.printf ("Unrecognised state %d\r\n", mode.rd(COMM_SRC)); // set error flag instead here + pc.printf ("Unrecognised state %d\r\n", user_settings.rd(COMM_SRC)); // set error flag instead here ESC_Error.set (FAULT_UNRECOGNISED_STATE, 1); } break; - } // endof switch (mode_bytes[COMM_SRC]) { - -#ifdef USING_DC_MOTORS - dc_motor_kicker_timer.reset (); -#endif - MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second - MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM + } // endof switch (user_settings_bytes[COMM_SRC]) { -#ifdef USING_DC_MOTORS - if (MotorA.dc_motor) { -// MotorA.raw_V_pwm (1); - MotorA.low_side_on (); - } - if (MotorB.dc_motor) { -// MotorB.raw_V_pwm (1); - MotorB.low_side_on (); - } - if (MotorA.dc_motor || MotorB.dc_motor) { -// motors_restore.attach_us (&restorer, ttime); - motors_restore.attach_us (&restorer, 25); - } -#endif -#define SERVO_OUT_TEST -#ifdef SERVO_OUT_TEST +//#define SERVO_OUT_TEST +//#ifdef SERVO_OUT_TEST // static double servo_angle = 0.0; // For testing servo outs // servo out test here December 2018 - - // Tests for pulse width and repetition rates being believable signal from radio control - if (RC_chan_1.validate_rx()) - Servo1 = RC_chan_1.normalised(); - if (RC_chan_2.validate_rx()) - Servo2 = RC_chan_2.normalised(); -// RC_chan_2.validate_rx(); - - /* servo_angle += 0.01; if (servo_angle > TWOPI) @@ -630,31 +584,23 @@ Servo2 = ((cos(servo_angle)+1.0) / 2.0); */ // Yep! Both servo outs work lovely December 2018 -#endif +//#endif if (flag_8Hz) { // do slower stuff flag_8Hz = false; - LED = !LED; // Toggle LED on board, should be seen to fast flash + LED = !LED; // Toggle green LED on board, should be seen to fast flash if (WatchDogEnable) { WatchDog--; if (WatchDog < 1) { // Deal with WatchDog timer timeout here WatchDog = 0; - setVI (0.0, 0.0); // set motor volts and amps to zero -// com2.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD - pc.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this + setVI_both (0.0, 0.0); // set motor volts and amps to zero + pc.printf ("TIMEOUT %c\r\n", user_settings.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this } // End of dealing with WatchDog timer timeout } - Update_Current_Scaler (); eighth_sec_count++; if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts eighth_sec_count = 0; ESC_Error.report_any (false); // retain = false - reset error after reporting once - /* if (temp_sensor_exists) { - double tmprt = (double) last_temp_count; - tmprt /= 16.0; - tmprt -= 50.0; - pc.printf ("Temp %.2f\r\n", tmprt); - }*/ } } // End of if(flag_8Hz) } // End of main programme loop