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:
- 3:ecb00e0e8d68
- Parent:
- 2:04761b196473
- Child:
- 4:21d91465e4b1
--- a/main.cpp Sat Mar 10 10:11:07 2018 +0000 +++ b/main.cpp Sun Mar 18 08:17:56 2018 +0000 @@ -47,6 +47,9 @@ 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, @@ -64,6 +67,9 @@ 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 @@ -109,14 +115,14 @@ // Pin 34 Port_B BWH DigitalOut T4 (PB_14); // Pin 35 DigitalOut T3 (PB_15); // Pin 36 -// BufferedSerial xb pins 37 Tx, 38 Rx -BufferedSerial xb (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module +// 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 ser3 pins 42 Tx, 43 Rx -BufferedSerial ser3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module +// 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 @@ -167,15 +173,18 @@ /* 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 - 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 bool sounder_on = false; -double test_pot = 0.0, test_amps = 0.0; // These used in knifeandfork code testing only + +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 @@ -202,7 +211,7 @@ * 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 +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 @@ -308,30 +317,63 @@ /* 5 1 3 2 6 4 SENSOR SEQUENCE -1 1 1 1 0 0 0 ---___---___ -2 0 0 1 1 1 0 __---___---_ -4 1 0 0 0 1 1 -___---___-- +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[] = { +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 + 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 Hall_tab_ptr, latest_pulses_per_sec; + uint32_t latest_pulses_per_sec, Hall_tab_ptr, direction, ppstmp; + bool moving_flag; const uint16_t * lut; FastPWM * maxV, * maxI; PortOut * Motor_Port; @@ -340,39 +382,40 @@ 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; -// PinName Hall1, Hall2, Hall3; + uint32_t Hindex[2], tickleon, encoder_error_cnt; motor () {} ; // Default constructor - motor (PortOut * , FastPWM * , FastPWM * , const uint16_t * , InterruptIn * H1, InterruptIn * H2, InterruptIn * H3) ; + 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 - bool set_mode (int); - 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; + 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, &MAH1, &MAH2, &MAH3); -motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, &MBH1, &MBH2, &MBH3); +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::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn * H1, InterruptIn * H2, InterruptIn * H3) // Constructor -{ - Hall1 =H1; - Hall2 =H2; - Hall3 =H3; +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 = mode = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking - Hindex = 0; - Hall_tab_ptr = 0; + 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 @@ -383,7 +426,26 @@ // 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 () { @@ -391,26 +453,13 @@ 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::high_side_off () { + uint16_t p = *Motor_Port; + p &= lut[32]; // KEEP_L_MASK_A or B + *Motor_Port = p; } void motor::current_calc () @@ -457,123 +506,96 @@ maxI->pulsewidth_ticks (a); // PWM } -uint32_t motor::pulses_per_sec () // call this once per main loop pass to keep count = edges per sec -{ - uint32_t tmp = Hall_total; - latest_pulses_per_sec = tmp - edge_count_table[Hall_tab_ptr]; - edge_count_table[Hall_tab_ptr] = tmp; +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++; -// mode &= 0x038L; // safety - *Motor_Port = lut[mode | Hindex]; + Hindex[1] = Hindex[0]; } -void MAH1r () -{ - MotorA.Hindex = 1; - if (MAH2 != 0) MotorA.Hindex |= 2; - if (MAH3 != 0) MotorA.Hindex |= 4; - MotorA.Hall_change (); -} -void MAH1f () -{ - MotorA.Hindex = 0; - if (MAH2 != 0) MotorA.Hindex |= 2; - if (MAH3 != 0) MotorA.Hindex |= 4; - MotorA.Hall_change (); -} -void MAH2r () +void MAH_isr () { - MotorA.Hindex = 2; - if (MAH1 != 0) MotorA.Hindex |= 1; - if (MAH3 != 0) MotorA.Hindex |= 4; - MotorA.Hall_change (); -} -void MAH2f () -{ - MotorA.Hindex = 0; - if (MAH1 != 0) MotorA.Hindex |= 1; - if (MAH3 != 0) MotorA.Hindex |= 4; - MotorA.Hall_change (); -} -void MAH3r () -{ - MotorA.Hindex = 4; - if (MAH1 != 0) MotorA.Hindex |= 1; - if (MAH2 != 0) MotorA.Hindex |= 2; - MotorA.Hall_change (); -} -void MAH3f () -{ - MotorA.Hindex = 0; - if (MAH1 != 0) MotorA.Hindex |= 1; - if (MAH2 != 0) MotorA.Hindex |= 2; + 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 MBH1r () -{ - MotorB.Hindex = 1; - if (MBH2 != 0) MotorB.Hindex |= 2; - if (MBH3 != 0) MotorB.Hindex |= 4; - MotorB.Hall_change (); -} -void MBH1f () -{ - MotorB.Hindex = 0; - if (MBH2 != 0) MotorB.Hindex |= 2; - if (MBH3 != 0) MotorB.Hindex |= 4; - MotorB.Hall_change (); -} -void MBH2r () +void MBH_isr () { - MotorB.Hindex = 2; - if (MBH1 != 0) MotorB.Hindex |= 1; - if (MBH3 != 0) MotorB.Hindex |= 4; - MotorB.Hall_change (); -} -void MBH2f () -{ - MotorB.Hindex = 0; - if (MBH1 != 0) MotorB.Hindex |= 1; - if (MBH3 != 0) MotorB.Hindex |= 4; - MotorB.Hall_change (); -} -void MBH3r () -{ - MotorB.Hindex = 4; - if (MBH1 != 0) MotorB.Hindex |= 1; - if (MBH2 != 0) MotorB.Hindex |= 2; - MotorB.Hall_change (); -} -void MBH3f () -{ - MotorB.Hindex = 0; - if (MBH1 != 0) MotorB.Hindex |= 1; - if (MBH2 != 0) MotorB.Hindex |= 2; + 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 @@ -583,9 +605,38 @@ MotorB.set_I_limit (i); } -void AtoD_reader () +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; @@ -614,6 +665,14 @@ 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 (); } } @@ -651,6 +710,23 @@ 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; @@ -658,20 +734,30 @@ 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); - - MBH1.rise (& MBH1r); - MBH1.fall (& MBH1f); - MBH2.rise (& MBH2r); - MBH2.fall (& MBH2f); - MBH3.rise (& MBH3r); - MBH3.fall (& MBH3f); - +*/ MAH1.mode (PullUp); MAH2.mode (PullUp); MAH3.mode (PullUp); @@ -687,6 +773,10 @@ 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"); @@ -708,6 +798,7 @@ T4 = 0; T5 = 0; T6 = 0; +// MotPtr[0]->set_mode (REGENBRAKE); MotorA.set_mode (REGENBRAKE); MotorB.set_mode (REGENBRAKE); // MotorA.set_mode (FORWARD); @@ -718,6 +809,26 @@ 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 @@ -727,22 +838,20 @@ 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 + 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; - 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); +// 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