Jon Freeman
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
Revision 3:ecb00e0e8d68, committed 2018-03-18
- Comitter:
- JonFreeman
- Date:
- Sun Mar 18 08:17:56 2018 +0000
- Parent:
- 2:04761b196473
- Child:
- 4:21d91465e4b1
- Commit message:
- Starting motors requires high-side mosfet drivers being enabled. Auto tickleup functions now included to switch high sides off and on again to charge high side supply capacitors (now 2u2, up from 100n)
Changed in this revision
--- a/24LC64_eeprom.cpp Sat Mar 10 10:11:07 2018 +0000
+++ b/24LC64_eeprom.cpp Sun Mar 18 08:17:56 2018 +0000
@@ -3,7 +3,7 @@
#include "BufferedSerial.h"
extern BufferedSerial pc;
extern I2C i2c;
- // Code for 24LC64 eeprom
+ // Code for 24LC64 8k x 8 bit eeprom
// Code based on earlier work using memory FM24W256, also at i2c address 0xa0;
const int addr_rd = 0xa1; // set bit 0 for read, clear bit 0 for write
@@ -20,7 +20,7 @@
i2cfree = true;
else
// Thread::wait(1); // 1ms
- wait (1);
+ wait_ms (1);
}
// pc.printf ("ack_poll, count = %d, i2cfree = %s\r\n", poll_count, i2cfree ? "true" : "false");
return i2cfree;
@@ -50,6 +50,7 @@
pc.printf ("Length out of range %d in wr_24LC64\r\n", length);
return false;
}
+ ack_poll ();
if (!set_24LC64_internal_address (start_addr)) {
pc.printf ("In wr_24LC64, Believe Device present, failed in writing 2 mem addr bytes %d\r\n", err);
return false;
--- a/DualBLS.h Sat Mar 10 10:11:07 2018 +0000
+++ b/DualBLS.h Sun Mar 18 08:17:56 2018 +0000
@@ -4,3 +4,6 @@
FORWARD = 8,
REVERSE = 16,
REGENBRAKE = 24;
+
+const double PI = 4.0 * atan(1.0),
+ TWOPI = 8.0 * atan(1.0);
--- a/cli_nortos.cpp Sat Mar 10 10:11:07 2018 +0000
+++ b/cli_nortos.cpp Sun Mar 18 08:17:56 2018 +0000
@@ -4,150 +4,153 @@
#include "DualBLS.h"
using namespace std;
-typedef double fl_typ; //
+extern int I_Am () ; // Returns boards id number as ASCII char '0', '1' etc. Code for Broadcast = '\r'
+//typedef double fl_typ; //
-const int MAX_PARAMS = 10;
+const int BROADCAST = '\r';
+const int MAX_PARAMS = 20;
struct parameters {
// int32_t times[numof_menu_items];
- int32_t times[50];
- int32_t position_in_list, last_time, numof_dbls;
+// int32_t times[50];
+ int32_t position_in_list, last_time, numof_dbls, target_unit;
double dbl[MAX_PARAMS];
+ bool respond;
} ;
// WithOUT RTOS
-extern BufferedSerial xb, pc;
-//extern BufferedSerial bt;
+extern BufferedSerial com2, pc;
//extern void set_I_limit (double p) ; // Sets max motor current
//extern void set_V_limit (double p) ; // Sets max motor voltage
extern void send_test () ;
-//extern void assemble_rx_frame () ;
-//extern void read_pulses (uint32_t * ) ;
-//extern void apply_brake (double b) ;
-//extern uint32_t Watch_Dog ;
extern void setVI (double v, double i) ;
-/*void pcp (char * toprint) {
- pc.printf (toprint);
- return;
-}*/
+BufferedSerial * com;
void null_cmd (struct parameters & a) {
- pc.printf ("At null_cmd, parameters : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
+ com->printf ("At null_cmd, parameters : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
}
-extern void tickle () ;
+/*extern void tickleboth () ;
void ti_cmd (struct parameters & a) {
- pc.printf ("At tickle\r\n");
- tickle ();
-}
+ com->printf ("At tickle\r\n");
+ tickleboth ();
+}*/
-void rd_cmd (struct parameters & a) { // Reading Hall pulse totals and clock() from bogie
- uint32_t rd[2];
- char t[36];
-// read_pulses (rd);
- sprintf (t, "P0=%d, P1=%d, clock=%d\n", rd[0], rd[1], clock());
- pc.printf (t);
-}
extern void mode_test (int mode, double val) ;
void fw_cmd (struct parameters & a) {
- pc.printf ("Forward : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
+ com->printf ("Forward : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
mode_test (FORWARD, 0.0);
}
void re_cmd (struct parameters & a) {
- pc.printf ("Reverse : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
+ com->printf ("Reverse : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
mode_test (REVERSE, 0.0);
}
void rb_cmd (struct parameters & a) { // Regen brake
double b = a.dbl[0] / 99.0;
- pc.printf ("Applying brake %.3f\r\n", b);
+ com->printf ("Applying brake %.3f\r\n", b);
mode_test (REGENBRAKE, b);
// apply_brake (b);
}
-/*void drive_cmd (struct parameters & a) { // Drive
- double drive = a.dbl[0] / 999.0;
+extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
+extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
+void erase_cmd (struct parameters & a) { // Sets eeprom contents to all 0xff. 256 pages of 32 bytes to do
+ char t[36];
+ for (int i = 0; i < 32; i++)
+ t[i] = 0xff;
+ for (int i = 0; i < 8191; i += 32) {
+ com->printf (".");
+ if (!wr_24LC64 (i, t, 32))
+ com->printf ("eeprom write prob\r\n");
+ }
}
-void coast_cmd (struct parameters & a) { // Coast
+/*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;
+} ;
+*/
+struct optpar {
+ int min, max, def;
+ const char * t;
+} ;
+struct optpar const option_list[] = {
+ {0, 1, 1, "MotorA direction 0 or 1"},
+ {0, 1, 0, "MotorB direction 0 or 1"},
+ {0, 1, 1, "gang 0 for separate control (robot mode), 1 for ganged loco bogie mode"},
+ {0, 2, 2, "Servo1 0, 1, 2 = Not used, Input, Output"},
+ {0, 2, 2, "Servo2 0, 1, 2 = Not used, Input, Output"},
+ {1, 5, 2, "Command source 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2"},
+ {0, 9, 0, "Alternative ID 0 to 9"},
+} ;
+
+void mode_cmd (struct parameters & a) { // With no params, reads eeprom contents. With params sets eeprom contents
+ char t[36];
+ const int numofopts = sizeof(option_list) / sizeof(struct optpar);
+ rd_24LC64 (0, t, 32);
+ com->printf ("Numof params=%d\r\n", a.numof_dbls);
+ for (int i = 0; i < numofopts; i++)
+ com->printf ("%2x\t%s\r\n", t[i], option_list[i].t);
+ if (a.numof_dbls == 0) { // Read present contents, do not write
+ com->printf ("That's it\r\n");
+ }
+ else { // Write new shit to eeprom
+ com->printf ("\r\n");
+ if (a.numof_dbls != numofopts) {
+ com->printf ("params required = %d, you offered %d\r\n", numofopts, a.numof_dbls);
+ }
+ else { // Have been passed correct number of parameters
+ int b;
+ com->printf("Ready to write params to eeprom\r\n");
+ for (int i = 0; i < numofopts; i++) {
+ b = (int)a.dbl[i]; // parameter value to check against limits
+ if ((b < option_list[i].min) || (b > option_list[i].max)) { // if parameter out of range
+ com->printf("Warning - Parameter = %d, out of range, setting to default %d\r\n", b, option_list[i].def);
+ b = option_list[i].def;
+ }
+ com->printf ("%2x\t%s\r\n", (t[i] = b), option_list[i].t);
+ }
+ wr_24LC64 (0, t, numofopts);
+ com->printf("Parameters set in eeprom\r\n");
+ }
+ }
+}
+/*void coast_cmd (struct parameters & a) { // Coast
}
*/
void hb_cmd (struct parameters & a) {
- pc.printf ("numof params = %d\r\n", a.numof_dbls);
- pc.printf ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
+ com->printf ("numof params = %d\r\n", a.numof_dbls);
+ com->printf ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
mode_test (HANDBRAKE, 0.0);
}
-//void wd_cmd (struct parameters & a) {
-// pc.printf ("Reset Watch Dog timer, was %d\r\n", Watch_Dog);
-// Watch_Dog = 0;
-//}
-
-/*void hall_cmd (struct parameters * a) { // Report back pulse totals from Hall sensors for both motors
- char buff[30];
- sprintf (buff, "Halls 0x%08lx 0x%08lx\r\n", Halls_A_accum, Halls_B_accum); // doesn't need to be hex
- pc.printf (buff);
-
- Halls_A_accum += 1369;
- Halls_B_accum += 11369;
-}*/
-
-/*void setpwm_cmd (struct parameters * a) {
- double pw = ((double)a[1].i) / 1000.0;
- pc.printf ("Setting Volt limit %.3f, %d\r\n", pw, a[1].i);
- set_V_limit (pw);
-}
-
-void setvref_cmd (struct parameters * a) {
- double pw = ((double)a[1].i) / 1000.0;
- pc.printf ("Setting Current limit %.3f, %d\r\n", pw, a[1].i);
- set_I_limit (pw);
-}*/
-
-/*
-void setmotpwm_cmd (struct parameters * a) {
- //a[1].i // first param
- pc.printf ("First %d, second %d\r\n", a[1].i, a[2].i);
-// set_motor_pwm (a[1].i, a[2].i);
-}
-//extern void set_fwd_rev (int direction) ;
-void set_fwd_cmd (struct parameters * a) {
-// set_fwd_rev (FWD);
-}
-void set_rev_cmd (struct parameters * a) {
-// set_fwd_rev (REV);
-}
-
-void set_speed_cmd (struct parameters * a) {
- pc.printf ("Speed entered %d\r\n", a[1].i);
-}
-
-void read_current_cmd (struct parameters * a) {
- pc.printf ("Read current\r\n");
-}
-*/
void menucmd (struct parameters & a);
-//void xb_cmd (struct parameters & a)
-//{
-// send_test();
-//}
-
-//extern void set_api_mode (bool mode) ;
void vi_cmd (struct parameters & a)
{
- pc.printf ("In setVI, setting V to %.2f, I %.2f\r\n", a.dbl[0], a.dbl[1]);
+ com->printf ("In setVI, setting V to %.2f, I %.2f\r\n", a.dbl[0], a.dbl[1]);
setVI (a.dbl[0] / 100.0, a.dbl[1] / 100.0);
}
-/*void at_cmd (struct parameters & a)
+void kd_cmd (struct parameters & a) // kick the watchdog
{
- xb.printf ("AT\r");
-// pc.printf ("AT %d\r\n", a[1].i);
-}*/
+}
+
+void who_cmd (struct parameters & a)
+{
+ int i = I_Am ();
+ if (I_Am() == a.target_unit)
+ com->printf ("Hi there, I am %c\r\n", a.target_unit);
+}
struct kb_command {
const char * cmd_word; // points to text e.g. "menu"
@@ -156,36 +159,39 @@
} ;
struct kb_command const command_list[] = {
- {"menu", "Lists available commands, same as ls", menucmd},
- {"ls", "Lists available commands, same as menu", menucmd},
-// {"sv", "set Volts pwm 0 to 999", setpwm_cmd},
-// {"si", "set Amps pwm 0 to 999", setvref_cmd},
-// {"ha", "read Hall pulse totals", hall_cmd},
- {"ti", "tickle to try to get mosfet driver charge pump primed", ti_cmd},
+ {"ls", "Lists available commands", menucmd},
+ {"?", "Lists available commands, same as ls", menucmd},
+// {"ti", "tickle to try to get mosfet driver charge pump primed", ti_cmd},
{"fw", "forward", fw_cmd},
{"re", "reverse", re_cmd},
{"rb", "regen brake 0 to 99 %", rb_cmd},
-// {"dr", "drive", drive_cmd},
-// {"co", "coast", coast_cmd},
{"hb", "hand brake", hb_cmd},
-// {"at", "AT", at_cmd},
{"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
+ {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
+ {"mode", "read or set params in eeprom", mode_cmd},
+ {"erase", "set eeprom contents to all 0xff", erase_cmd},
+ {"kd", "kick the dog", kd_cmd},
{"nu", "do nothing", null_cmd},
-
-// {"rev", "set motors in tother direction", set_rev_cmd},
-// {"s", "set speed", set_speed_cmd},
-// {"i", "Read motor currents", read_current_cmd},
};
const int numof_menu_items = sizeof(command_list) / sizeof(kb_command);
void menucmd (struct parameters & a)
{
- pc.printf("\r\n\nDouble Brushless Motor Driver 2018\r\nAt menucmd function - listing commands:-\r\n");
+ com->printf("\r\n\nDouble Brushless Motor Driver 2018\r\nAt menucmd function - listing commands:-\r\n");
for(int i = 0; i < numof_menu_items; i++)
- pc.printf("[%s]\t\t%s\r\n", command_list[i].cmd_word, command_list[i].explan);
- pc.printf("End of List of Commands\r\n");
+ com->printf("[%s]\t\t%s\r\n", command_list[i].cmd_word, command_list[i].explan);
+ com->printf("End of List of Commands\r\n");
}
+
+/*
+New - March 2018
+Using opto isolated serial port, paralleled up using same pair to multiple boards running this code.
+New feature - commands have optional prefix digit 0-9 indicating which unit message is addressed to.
+Commands without prefix digit - broadcast to all units, none to respond.
+Only units recognising its address from prefix digit may respond. This avoids bus contention.
+But for BROADCAST commands, '0' may respond on behalf of the group
+*/
//void command_line_interpreter (void const *argument)
void command_line_interpreter ()
{
@@ -193,67 +199,62 @@
static char cmd_line[MAX_CMD_LEN + 4];
static int cl_index = 0;
int ch;
-char * pEnd;
+char * pEnd, * cmd_line_ptr;
static struct parameters param_block ;
-// while (true) {
-// assemble_rx_frame () ; // check for anything coming from xbee
-// Watch_Dog++;
-/* while (bt.readable()) {
- ch = bt.getc();
-// bt.putc(ch);
- pc.printf ("%c", ch);
- }*/
- while (pc.readable()) {
- ch = tolower(pc.getc());
- // pc.printf("%c", ch);
+ com = &com2;
+// while (pc.readable()) {
+ while (com->readable()) {
+ ch = tolower(com->getc());
if (cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
- pc.printf ("Error!! Stupidly long cmd line\r\n");
+ com->printf ("Error!! Stupidly long cmd line\r\n");
cl_index = 0;
}
- if (ch == '\r' || ch >= ' ' && ch <= 'z')
- pc.printf("%c", ch);
- else { // Using <Ctrl>+ 'F', 'B' for Y, 'L', 'R' for X, 'U', 'D' for Z
- cl_index = 0; // 6 2 12 18 21 4
- pc.printf("[%d]", ch);
- //nudger (ch); // was used on cnc to nudge axes a tad
- }
if(ch != '\r') // was this the 'Enter' key?
cmd_line[cl_index++] = ch; // added char to command being assembled
else { // key was CR, may or may not be command to lookup
+ param_block.target_unit = BROADCAST; // Broadcast
+ cmd_line_ptr = cmd_line;
cmd_line[cl_index] = 0; // null terminate command string
if(cl_index) { // If have got some chars to lookup
int i, wrdlen;
+ if (isdigit(cmd_line[0])) { // Look for command with prefix digit
+ cmd_line_ptr++; // point past identified digit prefix
+ param_block.target_unit = cmd_line[0]; // '0' to '9'
+ //com->printf ("Got prefix %c\r\n", cmd_line[0]);
+ }
for (i = 0; i < numof_menu_items; i++) { // Look for input match in command list
wrdlen = strlen(command_list[i].cmd_word);
- if(strncmp(command_list[i].cmd_word, cmd_line, wrdlen) == 0 && !isalpha(cmd_line[wrdlen])) { // If match found
+ if(strncmp(command_list[i].cmd_word, cmd_line_ptr, wrdlen) == 0 && !isalpha(cmd_line_ptr[wrdlen])) { // If match found
for (int k = 0; k < MAX_PARAMS; k++) {
param_block.dbl[k] = 0.0;
}
param_block.position_in_list = i;
param_block.last_time = clock ();
param_block.numof_dbls = 0;
- pEnd = cmd_line + wrdlen;
+ pEnd = cmd_line_ptr + wrdlen;
while (*pEnd) { // Assemble all numerics as doubles
param_block.dbl[param_block.numof_dbls++] = strtod (pEnd, &pEnd);
while (*pEnd && !isdigit(*pEnd) && '-' != *pEnd && '+' != *pEnd) {
pEnd++;
}
}
- pc.printf ("\r\n");
- for (int k = 0; k < param_block.numof_dbls; k++)
- pc.printf ("Read %.3f\r\n", param_block.dbl[k]);
- param_block.times[i] = clock();
+ //com->printf ("\r\n"); // Not allowed as many may output this.
+ //for (int k = 0; k < param_block.numof_dbls; k++)
+ // com->printf ("Read %.3f\r\n", param_block.dbl[k]);
+// param_block.times[i] = clock();
+ if ((param_block.target_unit == BROADCAST) && (I_Am() == '0'))
+ param_block.respond = true;
command_list[i].f(param_block); // execute command
i = numof_menu_items + 1; // to exit for loop
} // end of match found
} // End of for numof_menu_items
if(i == numof_menu_items)
- pc.printf("No Match Found for CMD [%s]\r\n", cmd_line);
+ com->printf("No Match Found for CMD [%s]\r\n", cmd_line);
} // End of If have got some chars to lookup
- pc.printf("\r\n>");
+ //com->printf("\r\n>");
cl_index = 0;
} // End of else key was CR, may or may not be command to lookup
- } // End of while (pc.readable())
+ } // End of while (com->readable())
// Thread::wait(20); // Using RTOS on this project
// }
}
--- 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