Jon Freeman
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