STM3 ESC dual brushless motor controller. 10-60v, motor power rating tiny to kW. Ganged or independent motor control As used in 'The Brute' locomotive - www.jons-workshop.com
Dependencies: mbed BufferedSerial Servo FastPWM
Diff: main.cpp
- Revision:
- 4:21d91465e4b1
- Parent:
- 3:ecb00e0e8d68
- Child:
- 5:ca86a7848d54
--- a/main.cpp Sun Mar 18 08:17:56 2018 +0000 +++ b/main.cpp Thu Apr 26 08:23:04 2018 +0000 @@ -2,11 +2,10 @@ #include "DualBLS.h" #include "BufferedSerial.h" #include "FastPWM.h" +#include "Servo.h" /* STM32F401RE - compile using NUCLEO-F401RE // PROJECT - Dual Brushless Motor Controller - March 2018. -DigitalIn nFault1 (); needs pullup -DigitalIn nFault2 (); needs pullup AnalogIn to read each motor current ____________________________________________________________________________________ @@ -24,15 +23,15 @@ */ // Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out -#define SERVO1_IN +//#define SERVO1_IN //#define SERVO1_OUT //#define SERVO2_IN -#define SERVO2_OUT +//#define SERVO2_OUT // Port A -> MotorA, Port B -> MotorB const uint16_t -AUL = 1 << 0, // Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) +AUL = 1 << 0, // Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) - GOOD, works well with auto-tickle of high side drivers AVL = 1 << 6, AWL = 1 << 4, @@ -122,7 +121,10 @@ //InterruptIn MotB_Hall (PA_8); // Pin 41 // Pin 41 Port_A AWH // BufferedSerial com3 pins 42 Tx, 43 Rx +//InterruptIn tryseredge (PA_9); BufferedSerial com3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module +// PA_9 is Tx. I wonder, can we also use InterruptIn on this pin to generate interrupts on tx bit transitions ? Let's find out ! +// No. // Feb 2018 Pins 44 and 45 now liberated, could use for serial or other uses //BufferedSerial extra_ser (PA_11, PA_12); // Pins 44, 45 tx, rx to XBee module @@ -144,19 +146,10 @@ I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom // Pin 60 BOOT0 -#ifdef SERVO1_IN +// Servo pins, 2 off. Configured as Input to read radio control receiver +// If used as servo output, code gives pin to 'Servo' - seems to work InterruptIn Servo1_i (PB_8); // Pin 61 to read output from rc rx -#endif -#ifdef SERVO1_OUT -FastPWM Servo1_o (PB_8, 2); //Prescaler 2. This is pwm 4/3 -#endif - -#ifdef SERVO2_IN InterruptIn Servo2_i (PB_9); // Pin 62 to read output from rc rx -#endif -#ifdef SERVO2_OUT -FastPWM Servo2_o (PB_9, 2); // Prescaler 2. This is pwm 4/4 -#endif // Pin 63 VSS // Pin 64 VDD @@ -168,7 +161,8 @@ MAIN_LOOP_ITERATION_Hz = 1000000 / MAIN_LOOP_REPEAT_TIME_US, CURRENT_SAMPLES_AVERAGED = 100, // Current is spikey. Reading smoothed by using average of this many latest current readings PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear - MAX_PWM_TICKS = SystemCoreClock / PWM_HZ; + MAX_PWM_TICKS = SystemCoreClock / PWM_HZ, + TICKLE_TIMES = 100 ; /* End of Please Do Not Alter these */ @@ -180,9 +174,8 @@ AtoD_Semaphore = 0; 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 sounder_on = false; -double angle = 0.0, angle_step = 0.000005, sinv, cosv; +double angle = 0.0, angle_step = 0.00005, sinv, cosv; //double test_pot = 0.0, test_amps = 0.0; // These used in knifeandfork code testing only /* End of Global variable declarations */ @@ -192,6 +185,14 @@ // Interrupt Service Routines +/*uint32_t edgeintcnt = 0; +void seredgerise () { edgeintcnt++; } +void seredgefall () { edgeintcnt++; } + +void seredgetest () { + com2.printf ("edgeintcnt = %d\r\n", edgeintcnt); + com3.printf ("%c", 0x55); +}*/ /** 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. @@ -224,33 +225,35 @@ 1. Input of pwm from model control Rx 2. Output pwm to drive model control servo */ -//enum {SERVO_UNUSED, SERVO_PWM_IN, SERVO_PWM_OUT} ; -class Servo -{ - FastPWM * out; +enum {SERVO_UNUSED, SERVO_IN, SERVO_OUT} ; + +class RControl_In +{ // Read servo style pwm input Timer t; - -public: - - bool rx_active; int32_t clock_old, clock_new; int32_t pulse_width_us, period_us; - Servo () { // Constructor +public: + RControl_In () { pulse_width_us = period_us = 0; - clock_old = clock_new = 0; - out = NULL; - rx_active = false; - } + com2.printf ("Setting up Radio_Congtrol_In\r\n"); + } ; bool validate_rx () ; void rise () ; void fall () ; - void out_pw_set (double); - void period_ticks (uint32_t); - void pulsewidth_ticks (uint32_t); - void set_out (FastPWM *); -} S1, S2; + uint32_t pulsewidth () ; + uint32_t period () ; + bool rx_active; +} ; -bool Servo::validate_rx () +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; @@ -259,14 +262,14 @@ return rx_active; } -void Servo::rise () +void RControl_In::rise () { t.stop (); period_us = t.read_us (); t.reset (); t.start (); } -void Servo::fall () +void RControl_In::fall () { pulse_width_us = t.read_us (); clock_old = clock_new; @@ -274,45 +277,10 @@ if ((clock_new - clock_old) < 4) rx_active = true; } -void Servo::out_pw_set (double outpw) -{ - if (outpw > 1.0) - outpw = 1.0; - if (outpw < 0.0) - outpw = 0.0; - outpw *= 1.2; // Change range to 1.2 ms to cover out pulse width range 0.9 to 2.1 ms - outpw += 0.9; // Bias to nom min servo range - pulsewidth_ticks ((uint32_t)(outpw * (SystemCoreClock / 2000))); -} -void Servo::period_ticks (uint32_t pt) -{ - if (out) out->period_ticks (pt); -} -void Servo::pulsewidth_ticks (uint32_t wt) -{ - if (out) out->pulsewidth_ticks (wt); -} -void Servo::set_out (FastPWM * op) -{ - out = op; -} + -void Servo1rise () -{ - S1.rise (); -} -void Servo1fall () -{ - S1.fall (); -} -void Servo2rise () -{ - S2.rise (); -} -void Servo2fall () -{ - S2.fall (); -} +Servo * Servos[2]; + //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0; /* 5 1 3 2 6 4 SENSOR SEQUENCE @@ -328,47 +296,26 @@ 0, AWV,AVU,AWU,AUW,AUV,AVW, 0, // 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, 0, // 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,0, // Regenerative Braking - KEEP_L_MASK_A, KEEP_H_MASK_A, // [32 and 33] - (uint16_t)((uint32_t)&MAH1 >> 16), (uint16_t)((uint32_t)&MAH1 & 0xffff), - (uint16_t)((uint32_t)&MAH2 >> 16), (uint16_t)((uint32_t)&MAH2 & 0xffff), - (uint16_t)((uint32_t)&MAH3 >> 16), (uint16_t)((uint32_t)&MAH3 & 0xffff) -// ((uint32_t)&MAH1), -// ((uint32_t)&MAH2), -// ((uint32_t)&MAH3) -// (uint16_t)((uint32_t)&MAH1 >> 16), (uint16_t)((uint32_t)&MAH1 & 0xffff), + KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33] } ; -const uint32_t A_t2[] = { - (uint32_t)&MAH1, - (uint32_t)&MAH2, - (uint32_t)&MAH3 +InterruptIn * AHarr[] = { + &MAH1, + &MAH2, + &MAH3 } ; -/*const uint16_t A_tabl[] = { // Table revised advancing magnetic pull angle - WORKS, but sucks more power for no apparent advantage - 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake - 0, AWU,AVW,AVU,AUV,AWV,AUW, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1 - 0, AVU,AUW,AVW,AWV,AWU,AUV, 0, // 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,0 // Regenerative Braking -} ;*/ const uint16_t B_tabl[] = { 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake 0, BWV,BVU,BWU,BUW,BUV,BVW, 0, // 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, 0, // 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,0, // Regenerative Braking - KEEP_L_MASK_B, KEEP_H_MASK_B, - (uint16_t)((uint32_t)&MBH1 >> 16), (uint16_t)((uint32_t)&MBH1 & 0xffff), - (uint16_t)((uint32_t)&MBH2 >> 16), (uint16_t)((uint32_t)&MBH2 & 0xffff), - (uint16_t)((uint32_t)&MBH3 >> 16), (uint16_t)((uint32_t)&MBH3 & 0xffff) -// ((uint32_t)&MBH1), -// ((uint32_t)&MBH2), -// ((uint32_t)&MBH3) + KEEP_L_MASK_B, KEEP_H_MASK_B } ; -const uint32_t B_t2[] = { - (uint32_t)&MBH1, - (uint32_t)&MBH2, - (uint32_t)&MBH3 +InterruptIn * BHarr[] = { + &MBH1, + &MBH2, + &MBH3 } ; -// void * vp[] = {(void*)MAH1, (void*)MAH2}; - class motor { uint32_t Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth @@ -386,7 +333,7 @@ uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored uint32_t Hindex[2], tickleon, encoder_error_cnt; motor () {} ; // Default constructor - motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, const uint32_t *) ; + 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 @@ -400,12 +347,12 @@ void high_side_off () ; } ; //MotorA, MotorB, or even Motor[2]; -motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, A_t2); -motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, B_t2); +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, const uint32_t * lut32ptr) // Constructor +motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor { // Constructor maxV = _maxV_; maxI = _maxI_; @@ -434,12 +381,9 @@ 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 = (InterruptIn *)(((uint32_t)lut[34] << 16) | (uint32_t)lut[35]); - Hall2 = (InterruptIn *)(((uint32_t)lut[36] << 16) | (uint32_t)lut[37]); - Hall3 = (InterruptIn *)(((uint32_t)lut[38] << 16) | (uint32_t)lut[39]); -// Hall1 = (InterruptIn *)lut32ptr[0]; -// Hall1 = (InterruptIn *)lut32ptr[1]; -// Hall1 = (InterruptIn *)lut32ptr[2]; + Hall1 = Hall[0]; + Hall2 = Hall[1]; + Hall3 = Hall[2]; } void motor::direction_set (int dir) { @@ -510,7 +454,7 @@ { // Can also test for motor running or not here if (ppstmp == Hall_total) { moving_flag = false; // Zero Hall transitions since previous call - motor not moving - tickleon = 100; + tickleon = TICKLE_TIMES; } else { moving_flag = true; @@ -597,17 +541,25 @@ } void setVI (double v, double i) { -// void set_V_limit (double) ; // Sets max motor voltage -// void set_I_limit (double) ; // Sets max motor current + 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_V_limit (v); MotorB.set_I_limit (i); } void sincostest () { sinv = sin(angle); // to set speed and direction of MotorA cosv = cos(angle); // to set speed and direction of MotorB + Servos[0]->write ((sinv + 1.0) / 2.0); + Servos[1]->write ((cosv + 1.0) / 2.0); angle += angle_step; if (angle > TWOPI) angle -= TWOPI; @@ -617,21 +569,21 @@ MotorA.set_mode (REVERSE); sinv = -sinv; } - MotorA.set_V_limit (0.01 + (sinv / 8.0)); + MotorA.set_V_limit (0.01 + (sinv / 1.3)); if (cosv > 0.0) MotorB.set_mode (FORWARD); else { MotorB.set_mode (REVERSE); cosv = -cosv; } - MotorB.set_V_limit (0.01 + (cosv / 8.0)); + MotorB.set_V_limit (0.01 + (cosv / 1.3)); } 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; - sincostest (); +// sincostest (); if (MotorA.tickleon) MotorA.high_side_off (); @@ -727,17 +679,21 @@ return i | '0'; } + int main() { - int j = 0; + int eighth_sec_count = 0; uint32_t Apps, Bpps; - MotA = 0; // Motor drive ports A and B + MotA = 0; // Output all 0s to Motor drive ports A and B MotB = 0; - MotPtr[0] = &MotorA; + MotPtr[0] = &MotorA; // Pointers to motor class objects MotPtr[1] = &MotorB; - MAH1.rise (& MAH_isr); +// tryseredge.rise (&seredgerise); +// tryseredge.fall (&seredgefall); + + MAH1.rise (& MAH_isr); // Set up interrupt vectors MAH1.fall (& MAH_isr); MAH2.rise (& MAH_isr); MAH2.fall (& MAH_isr); @@ -750,20 +706,16 @@ MBH2.fall (& MBH_isr); MBH3.rise (& MBH_isr); MBH3.fall (& MBH_isr); -/* - MAH1.rise (& MAH1r); - MAH1.fall (& MAH1f); - MAH2.rise (& MAH2r); - MAH2.fall (& MAH2f); - MAH3.rise (& MAH3r); - MAH3.fall (& MAH3f); -*/ + 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); + pc.printf ("\tAbandon Hope %d\r\n", LED ? 0 : 1); // 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 @@ -774,8 +726,8 @@ char buff[TXTBUFSIZ]; bool eerom_detected = false; pc.baud (9600); - com3.baud (9600); - com2.baud (9600); + com3.baud (1200); + com2.baud (19200); pc.printf ("RAM test - "); if (check_24LC64() != 0xa0) // searches for i2c devices, returns address of highest found @@ -801,19 +753,26 @@ // MotPtr[0]->set_mode (REGENBRAKE); MotorA.set_mode (REGENBRAKE); MotorB.set_mode (REGENBRAKE); -// MotorA.set_mode (FORWARD); -// MotorB.set_mode (FORWARD); MotorA.set_V_limit (0.9); MotorB.set_V_limit (0.9); MotorA.set_I_limit (0.5); MotorB.set_I_limit (0.5); - + 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; +/* // 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 @@ -829,6 +788,12 @@ break; } */ + MotorA.set_mode (FORWARD); + MotorB.set_mode (REVERSE); + MotorA.set_V_limit (0.2); + MotorB.set_V_limit (0.2); + MotorA.set_I_limit (0.5); + MotorB.set_I_limit (0.5); 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 @@ -843,15 +808,17 @@ if (flag_8Hz) { // do slower stuff flag_8Hz = false; LED = !LED; // Toggle LED on board, should be seen to fast flash - j++; - if (j > 6) { // Send some status info out of serial port every second and a bit or thereabouts - j = 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 (); MotorB.current_calc (); +// Apps += Bpps; // to kill compiler warning // pc.printf ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IA %d, IB %d, Arpm %d, Brpm %d\r\n", Read_BatteryVolts(), Read_DriverPot(), MotorA.read_Halls (), MotorB.read_Halls (), MotorA.I.ave, MotorB.I.ave, (Apps * 60) / 24, (Bpps * 60) / 24); - //pc.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); - pc.printf ("\tAangle_cnt %d\tAencoder_error_cnt %d", MotorA.angle_cnt, MotorA.encoder_error_cnt); - pc.printf ("\tBangle_cnt %d\tBencoder_error_cnt %d, J3 %d\r\n", MotorB.angle_cnt, MotorB.encoder_error_cnt, J3 == 0 ? 0 : 1); + 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); +// pc.printf ("\tAangle_cnt %d\tAencoder_error_cnt %d", MotorA.angle_cnt, MotorA.encoder_error_cnt); +// pc.printf ("\tBangle_cnt %d\tBencoder_error_cnt %d, J3 %d\r\n", MotorB.angle_cnt, MotorB.encoder_error_cnt, J3 == 0 ? 0 : 1); +// com2.printf ("RCI1 pw %d, RCI2 pw %d, 1per %d, 2per %d\r\n", RCI1.pulsewidth(), RCI2.pulsewidth(), RCI1.period(), RCI2.period()); } } // End of if(flag_8Hz) } // End of main programme loop