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
main.cpp
- Committer:
- JonFreeman
- Date:
- 2018-11-10
- Revision:
- 9:ac2412df01be
- Parent:
- 8:93203f473f6e
- Child:
- 10:e40d8724268a
File content as of revision 9:ac2412df01be:
#include "mbed.h" //#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h" #include "stm32f401xe.h" #include "DualBLS.h" #include "BufferedSerial.h" #include "FastPWM.h" #include "Servo.h" /* New 29th May 2018 - YET TO CODE FOR - Fwd/Rev line from possible remote hand control box has signal routed to T5 Also new LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable */ /* STM32F401RE - compile using NUCLEO-F401RE // PROJECT - Dual Brushless Motor Controller - Jon Freeman June 2018. AnalogIn to read each motor current ____________________________________________________________________________________ CONTROL PHILOSOPHY This Bogie 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. So much the better if the remote controller does issue sensible commands, but ... ____________________________________________________________________________________ */ #if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP #include "F401RE.h" // See here for warnings about Servo InterruptIn not working #endif #if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP #include "F446ZE.h" #endif /* Global variable declarations */ 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 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 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 AtoD_Semaphore = 0; int IAm; 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; bool eeprom_flag; // gets set according to 24LC674 being found or not bool mode_good_flag = false; char mode_bytes[36]; uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01 last_temp_count = 0; // global updated approx every 100ms after each LMT01 conversion completes // struct single_bogie_options bogie; double rpm2mph = 0.0; // gets calculated from eeprom mode entries if present /* 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 Ticker temperature_find_ticker; Timer temperature_timer; Timer dc_motor_kicker_timer; Timeout motors_restore; // Interrupt Service Routines void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec { static bool readflag = false; int t = temperature_timer.read_ms (); if ((t == 5) && (!readflag)) { last_temp_count = temp_sensor_count; temp_sensor_count = 0; readflag = true; } if (t == 6) readflag = false; } /** 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 { 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 if ((sys_timer & 0x03) == 0) flag_8Hz = true; } /** void ISR_voltage_reader () * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds * * AtoD_reader() called from convenient point in code to take readings outside of ISRs */ 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 } class RControl_In { // Read servo style pwm input Timer t; int32_t clock_old, clock_new; int32_t pulse_width_us, period_us; public: RControl_In () { pulse_width_us = period_us = 0; com2.printf ("Setting up Radio_Congtrol_In\r\n"); } ; bool validate_rx () ; void rise () ; void fall () ; uint32_t pulsewidth () ; uint32_t period () ; bool rx_active; } ; uint32_t RControl_In::pulsewidth () { return pulse_width_us; } uint32_t RControl_In::period () { return period_us; } bool RControl_In::validate_rx () { if ((clock() - clock_new) > 4) rx_active = false; else rx_active = true; return rx_active; } void RControl_In::rise () // These may not work as use of PortB as port may bugger InterruptIn use { t.stop (); period_us = t.read_us (); t.reset (); t.start (); } void RControl_In::fall () { pulse_width_us = t.read_us (); clock_old = clock_new; clock_new = clock(); if ((clock_new - clock_old) < 4) rx_active = true; } Servo * Servos[2]; //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0; /* 5 1 3 2 6 4 SENSOR SEQUENCE 1 1 1 1 0 0 0 ---___---___ Hall1 2 0 0 1 1 1 0 __---___---_ Hall2 4 1 0 0 0 1 1 -___---___-- Hall3 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 */ const uint16_t A_tabl[] = { // Origial table 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake 0, AWV,AVU,AWU,AUW,AUV,AVW,AUW, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 0, AVW,AUV,AUW,AWU,AVU,AWV,AWU, // 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 KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33] } ; InterruptIn * AHarr[] = { &MAH1, &MAH2, &MAH3 } ; const uint16_t B_tabl[] = { 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake 0, BWV,BVU,BWU,BUW,BUV,BVW,BUW, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 0, BVW,BUV,BUW,BWU,BVU,BWV,BWU, // 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 KEEP_L_MASK_B, KEEP_H_MASK_B } ; InterruptIn * BHarr[] = { &MBH1, &MBH2, &MBH3 } ; class motor { uint32_t Hall_total, visible_mode, inner_mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth uint32_t latest_pulses_per_sec, Hall_tab_ptr, direction, ppstmp; bool moving_flag; const uint16_t * lut; FastPWM * maxV, * maxI; PortOut * Motor_Port; InterruptIn * Hall1, * Hall2, * Hall3; public: bool dc_motor; struct currents { uint32_t max, min, ave; } I; int32_t angle_cnt; uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored uint32_t Hindex[2], tickleon, encoder_error_cnt; uint32_t RPM, PPS; double last_V, last_I; motor () {} ; // Default constructor motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ; void set_V_limit (double) ; // Sets max motor voltage void set_I_limit (double) ; // Sets max motor current void Hall_change () ; // Called in response to edge on Hall input pin void motor_set () ; // Energise Port with data determined by Hall sensors void direction_set (int) ; // sets 'direction' with bit pattern to eor with FORWARD or REVERSE in set_mode bool set_mode (int); // sets mode to HANDBRAKE, FORWARD, REVERSE or REGENBRAKE bool is_moving () ; // Returns true if one or more Hall transitions within last 31.25 milli secs void current_calc () ; // Updates 3 uint32_t I.min, I.ave, I.max uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs bool motor_is_brushless (); void high_side_off () ; void low_side_on () ; void raw_V_pwm (int); } ; //MotorA, MotorB, or even Motor[2]; motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr); motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, BHarr); //motor * MotPtr[8]; // Array of pointers to some number of motor objects motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor { // Constructor maxV = _maxV_; maxI = _maxI_; Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking latest_pulses_per_sec = 0; for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++) edge_count_table[i] = 0; if (lutptr != A_tabl && lutptr != B_tabl) pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n"); Hall_tab_ptr = 0; Motor_Port = P; pc.printf ("In motor constructor, Motor port = %lx\r\n", P); maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz maxI->period_ticks (MAX_PWM_TICKS + 1); maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20); maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30); visible_mode = REGENBRAKE; inner_mode = REGENBRAKE; lut = lutptr; Hindex[0] = Hindex[1] = read_Halls (); ppstmp = 0; tickleon = 0; direction = 0; angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction Hall1 = Hall[0]; Hall2 = Hall[1]; Hall3 = Hall[2]; PPS = 0; RPM = 0; last_V = last_I = 0.0; int x = read_Halls (); if (x == 7) dc_motor = true; else dc_motor = false; } bool motor::motor_is_brushless () { /* int x = read_Halls (); if (x < 1 || x > 6) return false; return true; */ return !dc_motor; } /** void motor::direction_set (int dir) { Used to set direction according to mode data from eeprom */ void motor::direction_set (int dir) { if (dir != 0) dir = FORWARD | REVERSE; // bits used in eor direction = dir; } int motor::read_Halls () { int x = 0; if (*Hall1 != 0) x |= 1; if (*Hall2 != 0) x |= 2; if (*Hall3 != 0) x |= 4; return x; } void motor::high_side_off () { uint16_t p = *Motor_Port; p &= lut[32]; // KEEP_L_MASK_A or B *Motor_Port = p; } void motor::low_side_on () { // uint16_t p = *Motor_Port; // p &= lut[31]; // KEEP_L_MASK_A or B *Motor_Port = lut[31]; } void motor::current_calc () { I.min = 0x0fffffff; // samples are 16 bit I.max = 0; I.ave = 0; uint16_t sample; for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) { sample = current_samples[i]; I.ave += sample; if (I.min > sample) I.min = sample; if (I.max < sample) I.max = sample; } I.ave /= CURRENT_SAMPLES_AVERAGED; } void motor::raw_V_pwm (int v) { if (v < 1) v = 1; if (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS; maxV->pulsewidth_ticks (v); } void motor::set_V_limit (double p) // Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting { if (p < 0.0) p = 0.0; if (p > 1.0) p = 1.0; last_V = p; // for read by diagnostics p *= 0.95; // need limit, ffi see MCP1630 data p = 1.0 - p; // because pwm is wrong way up maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts } void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level { int a; if (p < 0.0) p = 0.0; if (p > 1.0) p = 1.0; last_I = p; a = (int)(p * MAX_PWM_TICKS); if (a > MAX_PWM_TICKS) a = MAX_PWM_TICKS; if (a < 0) a = 0; maxI->pulsewidth_ticks (a); // PWM } uint32_t motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec { // Can also test for motor running or not here if (dc_motor) return 0; if (ppstmp == Hall_total) { // if (dc_motor || ppstmp == Hall_total) { moving_flag = false; // Zero Hall transitions since previous call - motor not moving tickleon = TICKLE_TIMES; } else { moving_flag = true; ppstmp = Hall_total; } latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr]; edge_count_table[Hall_tab_ptr] = ppstmp; Hall_tab_ptr++; if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz) Hall_tab_ptr = 0; PPS = latest_pulses_per_sec; RPM = (latest_pulses_per_sec * 60) / 24; return latest_pulses_per_sec; } bool motor::is_moving () { return moving_flag; } /** bool motor::set_mode (int m) Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE. If this causes change of mode, also sets V and I to zero. */ bool motor::set_mode (int m) { if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) { pc.printf ("Error in set_mode, invalid mode %d\r\n", m); return false; } if (visible_mode != m) { // Mode change, kill volts and amps to be safe set_V_limit (0.0); set_I_limit (0.0); visible_mode = m; } if (m == FORWARD || m == REVERSE) m ^= direction; inner_mode = m; // idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom return true; } void motor::Hall_change () { const int32_t delta_theta_lut[] = { // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7 } ; int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]]; if (delta_theta == 0) encoder_error_cnt++; else angle_cnt += delta_theta; *Motor_Port = lut[inner_mode | Hindex[0]]; // changed mode to inner_mode 27/04/18 Hall_total++; Hindex[1] = Hindex[0]; } void report_motor_types () // not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise { pc.printf ("Mot A is %s, Mot B is %s\r\n", MotorA.motor_is_brushless() ? "Brushless":"DC", MotorB.motor_is_brushless() ? "Brushless":"DC"); } 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 } void MAH_isr () { uint32_t x = 0; MotorA.high_side_off (); // T3 = !T3; if (MAH1 != 0) x |= 1; if (MAH2 != 0) x |= 2; if (MAH3 != 0) x |= 4; MotorA.Hindex[0] = x; // New in [0], old in [1] MotorA.Hall_change (); } void MBH_isr () { uint32_t x = 0; MotorB.high_side_off (); if (MBH1 != 0) x |= 1; if (MBH2 != 0) x |= 2; if (MBH3 != 0) x |= 4; MotorB.Hindex[0] = x; MotorB.Hall_change (); } // End of Interrupt Service Routines void motor::motor_set () { Hindex[0] = read_Halls (); *Motor_Port = lut[inner_mode | Hindex[0]]; } void setVI (double v, double i) { MotorA.set_V_limit (v); // Sets max motor voltage MotorA.set_I_limit (i); // Sets max motor current MotorB.set_V_limit (v); MotorB.set_I_limit (i); } void setV (double v) { MotorA.set_V_limit (v); MotorB.set_V_limit (v); } void setI (double i) { MotorA.set_I_limit (i); MotorB.set_I_limit (i); } void read_RPM (uint32_t * dest) { dest[0] = MotorA.RPM; dest[1] = MotorB.RPM; } void read_PPS (uint32_t * dest) { dest[0] = MotorA.PPS; dest[1] = MotorB.PPS; } void read_last_VI (double * d) // only for test from cli { d[0] = MotorA.last_V; d[1] = MotorA.last_I; d[2] = MotorB.last_V; d[3] = MotorB.last_I; } /** void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr Not part of ISR */ void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr { static uint32_t i = 0, tab_ptr = 0; // if (MotorA.dc_motor) { // MotorA.low_side_on (); // } // else { if (MotorA.tickleon) MotorA.high_side_off (); // } // if (MotorB.dc_motor) { // MotorB.low_side_on (); // } // else { if (MotorB.tickleon) MotorB.high_side_off (); // } if (AtoD_Semaphore > 20) { pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore); AtoD_Semaphore = 20; } while (AtoD_Semaphore > 0) { AtoD_Semaphore--; // Code here to sequence through reading voltmeter, demand pot, ammeters switch (i) { // case 0: volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading volt_reading >>= 1; // Result = Result / 2 break; // i.e. Very simple digital low pass filter case 1: driverpot_reading += Ain_DriverPot.read_u16 (); driverpot_reading >>= 1; break; case 2: MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); // break; case 3: MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); // if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter tab_ptr = 0; break; } i++; // prepare to read the next input in response to the next interrupt if (i > 3) i = 0; } // end of while (AtoD_Semaphore > 0) { if (MotorA.tickleon) { // if (MotorA.dc_motor || MotorA.tickleon) { MotorA.tickleon--; MotorA.motor_set (); // Reactivate any high side switches turned off above } if (MotorB.tickleon) { // if (MotorB.dc_motor || MotorB.tickleon) { MotorB.tickleon--; MotorB.motor_set (); } } /** double Read_DriverPot () * 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_DriverPot () { return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0 } double Read_BatteryVolts () { return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct ! } void read_supply_vi (double * val) // called from cli { val[0] = MotorA.I.ave; val[1] = MotorB.I.ave; val[2] = Read_BatteryVolts (); } void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb { MotorA.set_mode (mode); MotorB.set_mode (mode); if (mode == 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); } } extern void setup_comms () ; extern void command_line_interpreter_pc () ; extern void command_line_interpreter_loco () ; extern int check_24LC64 () ; // Call from near top of main() to init i2c bus extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ; extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ; /*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes uint8_t MotA_dir, // 0 or 1 MotB_dir, // 0 or 1 gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode serv1, // 0, 1, 2 = Not used, Input, Output serv2, // 0, 1, 2 = Not used, Input, Output cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2 last; } ; */ int I_Am () // Returns boards id number as ASCII char { return IAm; } double mph (int rpm) { if (mode_good_flag) { return rpm2mph * (double) rpm; } return -1.0; } 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 (); } } //int ttime = 3; // resettable via cli 'stt', used to determine suitable force low on period for driving dc motor bool worth_the_bother (double a, double b) { double c = a - b; if (c < 0.0) c = 0.0 - c; if (c > 0.02) { // pc.printf ("%.3f\r\n", c); return true; } return false; } void prscfuck (int v) { /* int prescaler ( int value ) Set the PWM prescaler. The period of all PWM pins on the same PWM unit have to be reset after using this! Parameters: value - The required prescaler. Special values: 0 = lock current prescaler, -1 = use dynamic prescaler return - The prescaler which was set (can differ from requested prescaler if not possible) Definition at line 82 of file FastPWM_common.cpp. */ if (v < 1) v = 1; int w = A_MAX_V_PWM.prescaler (v); pc.printf ("Attempt setting prescaler %d returned %d\r\n", v, w); } enum { HAND_CONT_STATE_BEGIN, HAND_CONT_STATE_POWER_CYCLE_TO_EXIT, HAND_CONT_STATE_INTO_BRAKING, HAND_CONT_STATE_INTO_DRIVING, HAND_CONT_STATE_BRAKING, HAND_CONT_STATE_DRIVING } ; void hand_control_state_machine () { static int new_hand_controller_state = HAND_CONT_STATE_BEGIN; // static int old_hand_controller_state = HAND_CONT_STATE_BEGIN; static int old_hand_controller_direction = T5, t = 0; // Nov 2018 confirms Rob and Quentin obs, direction read at powerup static double brake_effort, drive_effort, pot_position, old_pot_position = 0.0; if (T5 != old_hand_controller_direction) { // 1 Forward, 0 Reverse pc.printf ("Direction change! Power off then on again to resume\r\n"); mode_set_both_motors (REGENBRAKE, 1.0); // old_hand_controller_state = new_hand_controller_state; new_hand_controller_state = HAND_CONT_STATE_POWER_CYCLE_TO_EXIT; } pot_position = Read_DriverPot (); // gets to here on first pass before pot has been read switch (new_hand_controller_state) { case HAND_CONT_STATE_BEGIN: pot_position = Read_DriverPot (); if (t++ > 2 && pot_position < 0.02) { pc.printf ("In BEGIN, pot %.2f\r\n", pot_position); new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING; } break; case HAND_CONT_STATE_POWER_CYCLE_TO_EXIT: break; case HAND_CONT_STATE_INTO_BRAKING: brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; mode_set_both_motors (REGENBRAKE, brake_effort); old_pot_position = pot_position; new_hand_controller_state = HAND_CONT_STATE_BRAKING; pc.printf ("Brake\r\n"); break; case HAND_CONT_STATE_INTO_DRIVING: new_hand_controller_state = HAND_CONT_STATE_DRIVING; pc.printf ("Drive\r\n"); if (T5) mode_set_both_motors (FORWARD, 0.0); // sets both motors else mode_set_both_motors (REVERSE, 0.0); break; case HAND_CONT_STATE_BRAKING: if (pot_position > Pot_Brake_Range) // escape braking into drive new_hand_controller_state = HAND_CONT_STATE_INTO_DRIVING; else { if (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 (REGENBRAKE, brake_effort); // pc.printf ("Brake %.2f\r\n", brake_effort); } } break; case HAND_CONT_STATE_DRIVING: if (pot_position < Pot_Brake_Range) // escape braking into drive new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING; else { if (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 ??? pc.printf ("Drive %.2f\r\n", drive_effort); } } break; default: break; } // endof switch (hand_controller_state) { } int main() { int eighth_sec_count = 0; MotA = 0; // Output all 0s to Motor drive ports A and B MotB = 0; // MotPtr[0] = &MotorA; // Pointers to motor class objects // MotPtr[1] = &MotorB; Temperature_pin.fall (&temp_sensor_isr); Temperature_pin.mode (PullUp); MAH1.rise (& MAH_isr); // Set up interrupt vectors MAH1.fall (& MAH_isr); MAH2.rise (& MAH_isr); MAH2.fall (& MAH_isr); MAH3.rise (& MAH_isr); MAH3.fall (& MAH_isr); MBH1.rise (& MBH_isr); MBH1.fall (& MBH_isr); MBH2.rise (& MBH_isr); MBH2.fall (& MBH_isr); MBH3.rise (& MBH_isr); MBH3.fall (& MBH_isr); MAH1.mode (PullUp); MAH2.mode (PullUp); MAH3.mode (PullUp); MBH1.mode (PullUp); MBH2.mode (PullUp); MBH3.mode (PullUp); Servo1_i.mode (PullUp); Servo2_i.mode (PullUp); // 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 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960); // Done setting up system interrupt timers temperature_timer.start (); // const int TXTBUFSIZ = 36; // char buff[TXTBUFSIZ]; pc.baud (9600); com3.baud (1200); com2.baud (19200); setup_comms (); IAm = '0'; if (check_24LC64() != 0xa0) { // searches for i2c devices, returns address of highest found pc.printf ("Check for 24LC64 eeprom FAILED\r\n"); com2.printf ("Check for 24LC64 eeprom FAILED\r\n"); eeprom_flag = false; } else { // Found 24LC64 memory on I2C eeprom_flag = true; bool k; k = rd_24LC64 (0, mode_bytes, 32); if (!k) com2.printf ("Error reading from eeprom\r\n"); // int err = 0; for (int i = 0; i < numof_eeprom_options; i++) { if ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max)) { com2.printf ("EEROM error with %s\r\n", option_list[i].t); pc.printf ("EEROM error with %s\r\n", option_list[i].t); if (i == ID) { // need to force id to '0' pc.printf ("Bad board ID value %d, forcing to \'0\'\r\n"); mode_bytes[ID] = '0'; } // err++; } // else // com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t); } rpm2mph = 0.0; if (mode_bytes[WHEELGEAR] == 0) // avoid making div0 error mode_bytes[WHEELGEAR]++; // if (err == 0) { mode_good_flag = true; MotorA.direction_set (mode_bytes[MOTADIR]); MotorB.direction_set (mode_bytes[MOTBDIR]); IAm = mode_bytes[ID]; rpm2mph = 60.0 // to Motor Revs per hour; * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR]) // Wheel revs per hour * PI * ((double)mode_bytes[WHEELDIA] / 1000.0) // metres per hour * 39.37 / (1760.0 * 36.0); // miles per hour // } // Alternative ID 1 to 9 // com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]); } // 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 T6 = 0; // MotPtr[0]->set_mode (REGENBRAKE); MotorA.set_mode (REGENBRAKE); MotorB.set_mode (REGENBRAKE); setVI (0.9, 0.5); // prscfuck (PWM_PRESECALER_DEFAULT); // Test fucking with fastpwm prescaler Servos[0] = Servos[1] = NULL; // NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8); // Only works with unconditional inline code // However, setup code for Input by default, // This can be used to monitor Servo output also ! Servo Servo1 (PB_8) ; Servos[0] = & Servo1; Servo Servo2 (PB_9) ; Servos[1] = & Servo2; // pc.printf ("last_temp_count = %d\r\n", last_temp_count); // Has had time to do at least 1 conversion if ((last_temp_count > 160) && (last_temp_count < 2400)) // in range -40 to +100 degree C temp_sensor_exists = true; /* // Setup Complete ! Can now start main control forever loop. // March 16th 2018 thoughts !!! // Control from one of several sources and types as selected in options bytes in eeprom. // Control could be from e.g. Pot, Com2, Servos etc. // Suggest e.g. */ /* switch (mode_byte) { // executed once only upon startup case POT: while (1) { // forever loop call common_stuff (); ... } break; case COM2: while (1) { // forever loop call common_stuff (); ... } break; } */ // pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR); dc_motor_kicker_timer.start (); int old_hand_controller_direction = T5; if (mode_bytes[COMM_SRC] == 3) { // Read fwd/rev switch 'T5', PA15 on 401 pc.printf ("Pot control\r\n"); if (T5) mode_set_both_motors (FORWARD, 0.0); // sets both motors else mode_set_both_motors (REVERSE, 0.0); } pc.printf ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]); 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 command_line_interpreter_pc () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true command_line_interpreter_loco () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts } loop_flag = false; // Clear flag set by ticker interrupt handler switch (mode_bytes[COMM_SRC]) { // Look to selected source of driving command, act on commands from wherever case 0: // Invalid break; case 1: // COM1 - Primarily for testing, bypassed by command line interpreter break; case 2: // COM2 - Primarily for testing, bypassed by command line interpreter break; case 3: // Put all hand controller input stuff here hand_control_state_machine (); break; // endof hand controller stuff case 4: // Servo1 break; case 5: // Servo2 break; default: pc.printf ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]); break; } // endof switch (mode_bytes[COMM_SRC]) { dc_motor_kicker_timer.reset (); MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second MotorB.pulses_per_sec (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM // T4 = !T4; // toggle to hang scope on to verify loop execution // do stuff 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); } if (flag_8Hz) { // do slower stuff flag_8Hz = false; LED = !LED; // Toggle LED on board, should be seen to fast flash if (WatchDogEnable) { WatchDog--; if (WatchDog == 0) { // Deal with WatchDog timer timeout here setVI (0.0, 0.0); // set motor volts and amps to zero com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD } // End of dealing with WatchDog timer timeout if (WatchDog < 0) WatchDog = 0; } 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; MotorA.current_calc (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max MotorB.current_calc (); /* if (temp_sensor_exists) { double tmprt = (double) last_temp_count; tmprt /= 16.0; tmprt -= 50.0; pc.printf ("Temp %.2f\r\n", tmprt); }*/ // com2.printf ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IAmin %d, IAave %d, IAmax %d, IB %d, Arpm %d, Brpm %d\r\n", Read_BatteryVolts(), Read_DriverPot(), MotorA.read_Halls (), MotorB.read_Halls (), MotorA.I.min, MotorA.I.ave, MotorA.I.max, MotorB.I.ave, (Apps * 60) / 24, (Bpps * 60) / 24); } } // End of if(flag_8Hz) } // End of main programme loop }