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
main.cpp
- Committer:
- JonFreeman
- Date:
- 2018-03-18
- Revision:
- 3:ecb00e0e8d68
- Parent:
- 2:04761b196473
- Child:
- 4:21d91465e4b1
File content as of revision 3:ecb00e0e8d68:
#include "mbed.h" #include "DualBLS.h" #include "BufferedSerial.h" #include "FastPWM.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 ____________________________________________________________________________________ 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 ... ____________________________________________________________________________________ */ // 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_OUT //#define SERVO2_IN #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) AVL = 1 << 6, AWL = 1 << 4, AUH = 1 << 1, AVH = 1 << 7, AWH = 1 << 8, AUV = AUH | AVL, AVU = AVH | AUL, AUW = AUH | AWL, AWU = AWH | AUL, AVW = AVH | AWL, AWV = AWH | AVL, KEEP_L_MASK_A = AUL | AVL | AWL, KEEP_H_MASK_A = AUH | AVH | AWH, BRA = AUL | AVL | AWL, BUL = 1 << 0, BVL = 1 << 1, BWL = 1 << 2, BUH = 1 << 10, BVH = 1 << 12, BWH = 1 << 13, BUV = BUH | BVL, BVU = BVH | BUL, BUW = BUH | BWL, BWU = BWH | BUL, BVW = BVH | BWL, BWV = BWH | BVL, KEEP_L_MASK_B = BUL | BVL | BWL, KEEP_H_MASK_B = BUH | BVH | BWH, BRB = BUL | BVL | BWL, PORT_A_MASK = AUL | AVL | AWL | AUH | AVH | AWH, // NEW METHOD FOR DGD21032 MOSFET DRIVERS PORT_B_MASK = BUL | BVL | BWL | BUH | BVH | BWH; PortOut MotA (PortA, PORT_A_MASK); PortOut MotB (PortB, PORT_B_MASK); // Pin 1 VBAT NET +3V3 DigitalIn J3 (PC_13, PullUp);// Pin 2 Jumper pulls to GND, R floats Hi // Pin 3 PC14-OSC32_IN NET O32I // Pin 4 PC15-OSC32_OUT NET O32O // Pin 5 PH0-OSC_IN NET PH1 // Pin 6 PH1-OSC_OUT NET PH1 // Pin 7 NRST NET NRST AnalogIn Ain_DriverPot (PC_0); // Pin 8 Spare Analogue in, net SAIN fitted with external pull-down AnalogIn Ain_SystemVolts (PC_1); // Pin 9 AnalogIn Motor_A_Current (PC_2); // Pin 10 might as well use up WSRA stock here AnalogIn Motor_B_Current (PC_3); // Pin 11 // Pin 12 VSSA/VREF- NET GND // Pin 13 VDDA/VREF+ NET +3V3 // Pin 14 Port_A AUL // Pin 15 Port_A AUH // Pins 16, 17 BufferedSerial pc BufferedSerial pc (PA_2, PA_3, 512, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead // Pin 18 VSS NET GND // Pin 19 VDD NET +3V3 // Pin 20 Port_A AWL // Pin 21 DigitalOut led1(LED1); DigitalOut LED (PA_5); // Pin 21 // Pin 22 Port_A AVL // Pin 23 Port_A AVH InterruptIn MBH2 (PC_4); // Pin 24 InterruptIn MBH3 (PC_5); // Pin 25 // Pin 26 Port_B BUL // Pin 27 Port_B BVL // Pin 28 Port_B BWL // Pin 29 Port_B BUH // Pin 30 VCAP1 // Pin 31 VSS // Pin 32 VDD // Pin 33 Port_B BVH // Pin 34 Port_B BWH DigitalOut T4 (PB_14); // Pin 35 DigitalOut T3 (PB_15); // Pin 36 // BufferedSerial com2 pins 37 Tx, 38 Rx BufferedSerial com2 (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module FastPWM A_MAX_V_PWM (PC_8, 1), // Pin 39 pwm3/3 A_MAX_I_PWM (PC_9, 1); // pin 40, prescaler value pwm3/4 //InterruptIn MotB_Hall (PA_8); // Pin 41 // Pin 41 Port_A AWH // BufferedSerial com3 pins 42 Tx, 43 Rx BufferedSerial com3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module // 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 DigitalOut T2 (PA_11); // Pin 44 DigitalOut T1 (PA_12); // Pin 45 // Pin 46 SWDIO // Pin 47 VSS // Pin 48 VDD // Pin 49 SWCLK DigitalOut T5 (PA_15); // Pin 50 InterruptIn MAH1 (PC_10); // Pin 51 InterruptIn MAH2 (PC_11); // Pin 52 InterruptIn MAH3 (PC_12); // Pin 53 InterruptIn MBH1 (PD_2); // Pin 54 DigitalOut T6 (PB_3); // Pin 55 FastPWM B_MAX_V_PWM (PB_4, 1), // Pin 56 pwm3/3 B_MAX_I_PWM (PB_5, 1); // pin 57, prescaler value pwm3/4 I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom // Pin 60 BOOT0 #ifdef SERVO1_IN 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 // SYSTEM CONSTANTS /* Please Do Not Alter these */ const int VOLTAGE_READ_INTERVAL_US = 50, // Interrupts timed every 50 micro sec, runs around loop performing 1 A-D conversion per pass 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 PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear MAX_PWM_TICKS = SystemCoreClock / PWM_HZ; /* End of Please Do Not Alter these */ /* Global variable declarations */ volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US 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; 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 test_pot = 0.0, test_amps = 0.0; // These used in knifeandfork code testing only /* 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 // Interrupt Service Routines /** 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 } /* Servo - mutex uses : 0. Unused 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; Timer t; public: bool rx_active; int32_t clock_old, clock_new; int32_t pulse_width_us, period_us; Servo () { // Constructor pulse_width_us = period_us = 0; clock_old = clock_new = 0; out = NULL; rx_active = false; } 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; bool Servo::validate_rx () { if ((clock() - clock_new) > 4) rx_active = false; else rx_active = true; return rx_active; } void Servo::rise () { t.stop (); period_us = t.read_us (); t.reset (); t.start (); } void Servo::fall () { pulse_width_us = t.read_us (); clock_old = clock_new; clock_new = clock(); 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 (); } //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 */ const uint16_t A_tabl[] = { // Origial table 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake 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), } ; const uint32_t A_t2[] = { (uint32_t)&MAH1, (uint32_t)&MAH2, (uint32_t)&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) } ; const uint32_t B_t2[] = { (uint32_t)&MBH1, (uint32_t)&MBH2, (uint32_t)&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 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: 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; motor () {} ; // Default constructor motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, const uint32_t *) ; 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 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 * 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 { // 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); // if (P != PortA && P != PortB) // pc.printf ("Fatal in 'motor' constructor, Invalid Port\r\n"); // else // PortOut Motor_P (P, *mask); // PortA for motor A, PortB for motor B 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 = (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]; } 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::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::set_V_limit (double p) // Sets max motor voltage { if (p < 0.0) p = 0.0; if (p > 1.0) p = 1.0; // last_pwm = p; 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; 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 (ppstmp == Hall_total) { moving_flag = false; // Zero Hall transitions since previous call - motor not moving tickleon = 100; } 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; return latest_pulses_per_sec; } bool motor::is_moving () { return moving_flag; } 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 (m == FORWARD || m == REVERSE) m ^= direction; mode = m; 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[mode | Hindex[0]]; Hall_total++; Hindex[1] = Hindex[0]; } 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[mode | Hindex[0]]; } 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 sincostest () { sinv = sin(angle); // to set speed and direction of MotorA cosv = cos(angle); // to set speed and direction of MotorB angle += angle_step; if (angle > TWOPI) angle -= TWOPI; if (sinv > 0.0) MotorA.set_mode (FORWARD); else { MotorA.set_mode (REVERSE); sinv = -sinv; } MotorA.set_V_limit (0.01 + (sinv / 8.0)); if (cosv > 0.0) MotorB.set_mode (FORWARD); else { MotorB.set_mode (REVERSE); cosv = -cosv; } MotorB.set_V_limit (0.01 + (cosv / 8.0)); } 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 (); if (MotorA.tickleon) MotorA.high_side_off (); if (MotorB.tickleon) MotorB.high_side_off (); if (AtoD_Semaphore > 20) { pc.printf ("WARNING - silly semaphore count %d, limiting to sensible\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) { MotorA.tickleon--; MotorA.motor_set (); } if (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 / 65535.0; // Normalise 0.0 <= control pot <= 1.0 } double Read_BatteryVolts () { return (double) volt_reading / 951.0; // divisor fiddled to make voltage reading correct ! } double read_volts () // A test function { pc.printf ("pot = %.4f, System Voltage = %.2f\r\n", Read_DriverPot(), Read_BatteryVolts()); 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 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 int i = J3; if (i != 0) i = 1; return i | '0'; } int main() { int j = 0; uint32_t Apps, Bpps; MotA = 0; // Motor drive ports A and B MotB = 0; MotPtr[0] = &MotorA; MotPtr[1] = &MotorB; MAH1.rise (& MAH_isr); 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.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); 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 loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator // Done setting up system interrupt timers const int TXTBUFSIZ = 36; char buff[TXTBUFSIZ]; bool eerom_detected = false; pc.baud (9600); com3.baud (9600); com2.baud (9600); pc.printf ("RAM test - "); if (check_24LC64() != 0xa0) // searches for i2c devices, returns address of highest found pc.printf ("Check for 24LC64 eeprom FAILED\r\n"); else // i2c.write returned 0, think this means device responded with 'ACK', found it anyway eerom_detected = true; if (eerom_detected) { bool j, k; pc.printf ("ok\r\n"); static const char ramtst[] = "I found the man sir!"; j = wr_24LC64 (0x1240, (char*)ramtst, strlen(ramtst)); for (int i = 0; i < TXTBUFSIZ; i++) buff[i] = 0; // Clear buffer // need a way to check i2c busy - YES implemented ack_poll k = rd_24LC64 (0x1240, buff, strlen(ramtst)); pc.printf("Ram test returned [%s], wr ret'd [%s], rd ret'd [%s]\r\n", buff, j ? "true" : "false", k ? "true" : "false"); } T1 = 0; // As yet unused pins T2 = 0; T3 = 0; T4 = 0; T5 = 0; T6 = 0; // 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); // 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; } */ 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 () ; // 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 Apps = MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second Bpps = MotorB.pulses_per_sec (); T4 = !T4; // toggle to hang scope on to verify loop execution // do stuff 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; MotorA.current_calc (); MotorB.current_calc (); // 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); } } // End of if(flag_8Hz) } // End of main programme loop }