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:
- 2:04761b196473
- Parent:
- 1:0fabe6fdb55b
- Child:
- 3:ecb00e0e8d68
--- a/main.cpp Wed Mar 07 08:29:18 2018 +0000 +++ b/main.cpp Sat Mar 10 10:11:07 2018 +0000 @@ -161,10 +161,6 @@ MAIN_LOOP_REPEAT_TIME_US = 31250, // 31250 us, with TACHO_TAB_SIZE = 32 means tacho_ticks_per_time is tacho_ticks_per_second 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 - HANDBRAKE = 0, - FORWARD = 8, - REVERSE = 16, - REGENBRAKE = 24, PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear MAX_PWM_TICKS = SystemCoreClock / PWM_HZ; @@ -174,6 +170,7 @@ 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 + fast_sys_timer = 0, // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US 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 @@ -208,6 +205,7 @@ void ISR_voltage_reader () // This is Ticker Interrupt Service Routine - few us between readings { AtoD_Semaphore++; + fast_sys_timer++; // Just a handy measure of elapsed time for anything to use } @@ -332,19 +330,21 @@ class motor { - struct currents { - uint32_t max, min, ave; - } I; uint32_t Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth uint32_t Hall_tab_ptr, latest_pulses_per_sec; const uint16_t * lut; FastPWM * maxV, * maxI; PortOut * Motor_Port; + InterruptIn * Hall1, * Hall2, * Hall3; public: + struct currents { + uint32_t max, min, ave; + } I; uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored uint32_t Hindex; +// PinName Hall1, Hall2, Hall3; motor () {} ; // Default constructor - motor (PortOut * , FastPWM * , FastPWM * , const uint16_t * ) ; + motor (PortOut * , FastPWM * , FastPWM * , const uint16_t * , InterruptIn * H1, InterruptIn * H2, InterruptIn * H3) ; 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 @@ -352,13 +352,17 @@ void current_calc () ; uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec int read_Halls () ; + void tickle () ; } ; //MotorA, MotorB; -motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl); -motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl); +motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, &MAH1, &MAH2, &MAH3); +motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, &MBH1, &MBH2, &MBH3); -motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr ) // Constructor +motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn * H1, InterruptIn * H2, InterruptIn * H3) // Constructor { + Hall1 =H1; + Hall2 =H2; + Hall3 =H3; maxV = _maxV_; maxI = _maxI_; Hall_total = mode = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking @@ -383,7 +387,30 @@ } int motor::read_Halls () { - return Hindex; + int x = 0; + if (*Hall1 != 0) x |= 1; + if (*Hall2 != 0) x |= 2; + if (*Hall3 != 0) x |= 4; + Hindex = x; + return x; +// return Hindex; +} + +void motor::tickle () // Attempt to get mosfet driver to drive high side fets +{ + volatile int t; + for (int cnt = 0; cnt < 20; cnt++) { + *Motor_Port = 0; + t = fast_sys_timer; + int x = read_Halls () | mode; + pc.printf ("Ti"); + *Motor_Port = lut[x]; + pc.printf ("%d\t", t); + } +} + +void tickle () { + MotorA.tickle (); } void motor::current_calc () @@ -443,8 +470,10 @@ bool motor::set_mode (int m) { - if (m != HANDBRAKE && m != FORWARD && m != REVERSE && m !=REGENBRAKE) + if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) { + pc.printf ("Error in set_mode, invalid mode %d\r\n", m); return false; + } mode = m; return true; } @@ -452,9 +481,10 @@ void motor::Hall_change () { Hall_total++; - mode &= 0x038L; // safety - *Motor_Port = lut[mode + Hindex]; +// mode &= 0x038L; // safety + *Motor_Port = lut[mode | Hindex]; } + void MAH1r () { MotorA.Hindex = 1; @@ -544,14 +574,15 @@ // End of Interrupt Service Routines -void buggery_fuck () // simulate hall movement to observe port output bits -{ - /* MotorA.index++; - if (MotorA.index > 6) - MotorA.index = 1; - MotorA.Hall_change (); - */ +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); + MotorA.set_I_limit (i); + MotorB.set_V_limit (v); + MotorB.set_I_limit (i); } + void AtoD_reader () { static uint32_t i = 0, tab_ptr = 0; @@ -593,16 +624,12 @@ */ double Read_DriverPot () { -#ifdef KNIFEANDFORK - return test_pot; // may be altered using cli -#else return (double) driverpot_reading / 65535.0; // Normalise 0.0 <= control pot <= 1.0 -#endif } double Read_BatteryVolts () { - return (double) volt_reading / 1800.0; // divisor fiddled to make voltage reading correct ! + return (double) volt_reading / 951.0; // divisor fiddled to make voltage reading correct ! } double read_volts () // A test function @@ -611,6 +638,13 @@ return Read_BatteryVolts(); } +void mode_test (int mode, double val) { + MotorA.set_mode (mode); + MotorB.set_mode (mode); + if (mode == REGENBRAKE) { + + } +} extern void command_line_interpreter () ; extern int check_24LC64 () ; // Call from near top of main() to init i2c bus @@ -674,14 +708,14 @@ T4 = 0; T5 = 0; T6 = 0; -// MotorA.set_mode (HANDBRAKE); -// MotorB.set_mode (HANDBRAKE); - MotorA.set_mode (FORWARD); - MotorB.set_mode (FORWARD); - MotorA.set_V_limit (0.1); - MotorB.set_V_limit (0.0); - MotorA.set_I_limit (0.0); - MotorB.set_I_limit (0.0); + 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); // Setup Complete ! Can now start main control forever loop. @@ -694,23 +728,21 @@ Apps = MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second Bpps = MotorB.pulses_per_sec (); - buggery_fuck (); - // do stuff if (flag_8Hz) { // do slower stuff flag_8Hz = false; - LED = !LED; // Toggle LED on board, should be seen to fast flash + // 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; - uint8_t stat; - stat = PressureSensor.getFstatus(); -double pres = PressureSensor.getPressure (); -//double tmpr = PressureSensor.getTemperature (); -//double pres = PressureSensor.getTemperature (); -// pc.printf ("Apps %d, Bpps %d, sys_timer %d\r\n", Apps, Bpps, sys_timer); - pc.printf ("Apps %d, Bpps %d, sys_timer %d, status %d, pressure %.2f\r\n", Apps, Bpps, sys_timer, stat, pres); - // pc.printf ("V=%+.1f, I=%+.1f, CtrlOut %.3f, RPM %d, %s\r\n", Read_BatteryVolts(), AmpsReading, ControlOut, ReadEngineRPM(), engine_warm ? "Running mode" : "Startup mode"); + LED = !LED; // Toggle LED on board, should be seen to fast flash + MotorA.current_calc (); + MotorB.current_calc (); +// pc.printf ("Hello\r\n"); +// uint8_t stat; +// pc.printf ("Arpm %d, Brpm %d, sys_timer %d, HA %d, HB %d\r\n", (Apps * 60) / 24, (Bpps * 60) / 24, sys_timer, MotorA.read_Halls (), MotorB.read_Halls ()); +// pc.printf ("Apps %d, Bpps %d, sys_timer %d, HA %d, HB %d\r\n", Apps, Bpps, sys_timer, MotorA.read_Halls (), MotorB.read_Halls ()); + 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); } } // End of if(flag_8Hz) } // End of main programme loop