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 5:ca86a7848d54, committed 2018-05-29
- Comitter:
- JonFreeman
- Date:
- Tue May 29 16:36:34 2018 +0000
- Parent:
- 4:21d91465e4b1
- Child:
- 6:f289a49c1eae
- Commit message:
- Adding temperature sensor and fw\re input from possible hand held control box
Changed in this revision
--- a/DualBLS.h Thu Apr 26 08:23:04 2018 +0000
+++ b/DualBLS.h Tue May 29 16:36:34 2018 +0000
@@ -5,5 +5,34 @@
REVERSE = 16,
REGENBRAKE = 24;
+const int TIMEOUT_SECONDS = 30;
+
+/* 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),
+ TICKLE_TIMES = 100 ,
+ WATCHDOG_RELOAD = (TIMEOUT_SECONDS * 8); // WatchDog counter ticked down in 8Hz loop
+
+/* End of Please Do Not Alter these */
const double PI = 4.0 * atan(1.0),
TWOPI = 8.0 * atan(1.0);
+
+struct optpar {
+ int min, max, def; // min, max, default
+ const char * t; // description
+} ;
+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"},
+ {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, // defaults to '0' before eerom setup for first time
+} ;
+const int numofopts = sizeof(option_list) / sizeof (struct optpar);
+
--- a/cli_nortos.cpp Thu Apr 26 08:23:04 2018 +0000
+++ b/cli_nortos.cpp Tue May 29 16:36:34 2018 +0000
@@ -1,60 +1,85 @@
+// DualBLS2018_03
#include "mbed.h"
#include "BufferedSerial.h"
+
#include <cctype>
#include "DualBLS.h"
using namespace std;
extern int I_Am () ; // Returns boards id number as ASCII char '0', '1' etc. Code for Broadcast = '\r'
-//typedef double fl_typ; //
+extern int WatchDog;
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, target_unit;
+ int32_t position_in_list, // set but not used Apr 2018, contains i for i'th menu item
+// last_time, // gets reading from clock() ; not known to be useful or reliable
+ numof_dbls,
+ target_unit;
double dbl[MAX_PARAMS];
bool respond;
} ;
// WithOUT RTOS
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 setVI (double v, double i) ;
extern void setV (double v) ;
extern void setI (double i) ;
+extern void read_last_VI (double * val) ; // only for test from cli
BufferedSerial * com;
-void null_cmd (struct parameters & a) {
- com->printf ("At null_cmd, parameters : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
+void null_cmd (struct parameters & a)
+{
+ if (a.respond)
+ com->printf ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
+}
+
+extern void mode_set (int mode, double val) ;
+extern void read_supply_vi (double * val) ;
+
+void rdi_cmd (struct parameters & a) // read motor currents
+{
+ if (a.respond) {
+ double r[4];
+ read_supply_vi (r); // get MotorA.I.ave, MotorB.I.ave, Battery volts
+ com->printf ("rdi%.0f %.0f %.1f\r", r[0], r[1], r[2]); // Format good to be unpicked by cli in touch screen controller
+ }
}
-extern void mode_test (int mode, double val) ;
-
-void fw_cmd (struct parameters & a) {
- com->printf ("Forward : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
- mode_test (FORWARD, 0.0);
+void rvi_cmd (struct parameters & a) // read last normalised values sent to pwms
+{
+ if (a.respond) {
+ double r[6];
+ read_last_VI (r);
+ com->printf ("rvi%.2f %.2f %.2f %.2f\r", r[0], r[1], r[2], r[3]);
+ }
}
-void re_cmd (struct parameters & a) {
- com->printf ("Reverse : First %d, second %d\r\n", a.dbl[0], a.dbl[1]);
- mode_test (REVERSE, 0.0);
+void fw_cmd (struct parameters & a) // Forward command
+{
+ mode_set (FORWARD, 0.0);
}
-void rb_cmd (struct parameters & a) { // Regen brake
- double b = a.dbl[0] / 99.0;
- com->printf ("Applying brake %.3f\r\n", b);
- mode_test (REGENBRAKE, b);
+void re_cmd (struct parameters & a) // Reverse command
+{
+ mode_set (REVERSE, 0.0);
+}
+
+void rb_cmd (struct parameters & a) // Regen brake command
+{
+ double b = a.dbl[0] / 100.0;
+// com->printf ("Applying brake %.3f\r\n", b);
+ mode_set (REGENBRAKE, b);
// apply_brake (b);
}
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
+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;
@@ -74,91 +99,95 @@
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;
+void mode_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents
+{
+ if (a.target_unit == BROADCAST) {
+// com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
+ } else {
+ char t[36];
+ com->printf ("At mode_cmd with node %d\r\n", a.target_unit);
+ 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 (i == 6) // Alternative ID must be turned to ascii
+ b |= '0';
+ 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 ("0x%2x\t%s\r\n", (t[i] = b), option_list[i].t);
}
- 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");
}
- 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) {
- 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 hb_cmd (struct parameters & a)
+{
+ if (a.respond) {
+ 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_set (HANDBRAKE, 0.0);
+}
+
+extern void read_RPM (uint32_t * dest) ;
+void rpm_cmd (struct parameters & a) // to report e.g. RPM 1000 1000 ; speed for both motors
+{
+ if (a.respond) {
+ uint32_t dest[3];
+ read_RPM (dest);
+ com->printf ("rpm%d %d\r", dest[0], dest[1]);
+ }
}
void menucmd (struct parameters & a);
void vi_cmd (struct parameters & a)
{
- com->printf ("In setVI, setting V to %.2f, I %.2f\r\n", a.dbl[0], a.dbl[1]);
+// if (a.respond)
+// 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 v_cmd (struct parameters & a)
{
- com->printf ("In setV, setting V to %.2f\r\n", a.dbl[0]);
+// if (a.respond)
+// com->printf ("In setV, setting V to %.2f\r\n", a.dbl[0]);
setV (a.dbl[0] / 100.0);
}
void i_cmd (struct parameters & a)
{
- com->printf ("In setI, setting I to %.2f\r\n", a.dbl[0]);
+// if (a.respond)
+// com->printf ("In setI, setting I to %.2f\r\n", a.dbl[0]);
setI (a.dbl[0] / 100.0);
}
void kd_cmd (struct parameters & a) // kick the watchdog
{
+ WatchDog = WATCHDOG_RELOAD + (I_Am() & 0x0f);
+// com->printf ("Poked %d up Dog\r\n", WatchDog);
}
-/*extern void seredgetest () ;
-void s_cmd (struct parameters & a) // test use of InterruptIn on serialout
-{
- seredgetest () ;
-}*/
-
void who_cmd (struct parameters & a)
{
int i = I_Am ();
@@ -175,7 +204,6 @@
struct kb_command const command_list[] = {
{"ls", "Lists available commands", menucmd},
{"?", "Lists available commands, same as ls", menucmd},
-// {"s", "seredgetest", s_cmd},
{"fw", "forward", fw_cmd},
{"re", "reverse", re_cmd},
{"rb", "regen brake 0 to 99 %", rb_cmd},
@@ -186,91 +214,98 @@
{"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},
+ {"kd", "kick the dog, reloads WatchDog", kd_cmd},
+ {"rpm", "read motor pair speeds", rpm_cmd},
+ {"rvi", "read most recent values sent to pwms", rvi_cmd},
+ {"rdi", "read motor currents and power voltage", rdi_cmd},
{"nu", "do nothing", null_cmd},
};
const int numof_menu_items = sizeof(command_list) / sizeof(kb_command);
void menucmd (struct parameters & a)
{
- 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++)
- 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");
+ if (a.respond) {
+ 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++)
+ 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.
+Commands without prefix digit - broadcast to all units, all to obey but 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 ()
{
-const int MAX_CMD_LEN = 120;
-static char cmd_line[MAX_CMD_LEN + 4];
-static int cl_index = 0;
-int ch;
-char * pEnd, * cmd_line_ptr;
-static struct parameters param_block ;
- com = &com2;
-// while (pc.readable()) {
- while (com->readable()) {
- ch = tolower(com->getc());
- if (cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
- com->printf ("Error!! Stupidly long cmd line\r\n");
- cl_index = 0;
- }
- 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_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;
+ const int MAX_CMD_LEN = 120;
+ static char cmd_line[MAX_CMD_LEN + 4];
+ static int cl_index = 0;
+ int ch, IAm = I_Am();
+ char * pEnd, * cmd_line_ptr;
+ static struct parameters param_block ;
+ com = &com2;
+ while (com->readable()) {
+// ch = tolower(com->getc());
+ ch = com->getc();
+ if (cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
+ com->printf ("Error!! Stupidly long cmd line\r\n");
+ cl_index = 0;
+ }
+ 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; // Set to BROADCAST default once found command line '\r'
+ 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_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_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++;
}
- param_block.position_in_list = i;
- param_block.last_time = clock ();
- param_block.numof_dbls = 0;
- 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++;
- }
- }
- //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]);
+ }
+ //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)
- com->printf("No Match Found for CMD [%s]\r\n", cmd_line);
- } // End of If have got some chars to lookup
- //com->printf("\r\n>");
- cl_index = 0;
- } // End of else key was CR, may or may not be command to lookup
- } // End of while (com->readable())
+ param_block.respond = false;
+ if (((param_block.target_unit == BROADCAST) && (IAm == '0')) || (IAm == param_block.target_unit))
+ param_block.respond = true; // sorted 26/4/18
+ // All boards to obey BROADCAST command, only specific board to obey number prefixed command
+ if ((param_block.target_unit == BROADCAST) || (IAm == param_block.target_unit))
+ command_list[i].f(param_block); // execute command if addressed to this unit
+ i = numof_menu_items + 1; // to exit for loop
+ } // end of match found
+ } // End of for numof_menu_items
+ if(i == numof_menu_items)
+ com->printf("No Match Found for CMD [%s]\r\n", cmd_line);
+ } // End of If have got some chars to lookup
+ //com->printf("\r\n>");
+ cl_index = 0;
+ } // End of else key was CR, may or may not be command to lookup
+ } // End of while (com->readable())
// Thread::wait(20); // Using RTOS on this project
// }
}
--- a/main.cpp Thu Apr 26 08:23:04 2018 +0000
+++ b/main.cpp Tue May 29 16:36:34 2018 +0000
@@ -3,8 +3,15 @@
#include "BufferedSerial.h"
#include "FastPWM.h"
#include "Servo.h"
+
+/*
+New 29th May 2018 - YET TO CODE FOR - Fwd/Rev line from possible remote hand control box has signal routed to T5
+ Also new LMT01 temperature sensor routed to T1
+*/
+
+
/* STM32F401RE - compile using NUCLEO-F401RE
-// PROJECT - Dual Brushless Motor Controller - March 2018.
+// PROJECT - Dual Brushless Motor Controller - Jon Freeman April 2018.
AnalogIn to read each motor current
@@ -23,23 +30,18 @@
*/
// 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) - GOOD, works well with auto-tickle of high side drivers
-AVL = 1 << 6,
+AVL = 1 << 6, // These are which port bits connect to which mosfet driver
AWL = 1 << 4,
AUH = 1 << 1,
AVH = 1 << 7,
AWH = 1 << 8,
-AUV = AUH | AVL,
+AUV = AUH | AVL, // Each of 6 possible output energisations made up of one hi and one low
AVU = AVH | AUL,
AUW = AUH | AWL,
AWU = AWH | AUL,
@@ -49,9 +51,9 @@
KEEP_L_MASK_A = AUL | AVL | AWL,
KEEP_H_MASK_A = AUH | AVH | AWH,
-BRA = AUL | AVL | AWL,
+BRA = AUL | AVL | AWL, // All low side switches on (and all high side off) for braking
-BUL = 1 << 0,
+BUL = 1 << 0, // Likewise for MotorB but different port bits on different port
BVL = 1 << 1,
BWL = 1 << 2,
@@ -74,7 +76,7 @@
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 MotA (PortA, PORT_A_MASK); // Activate output ports to motor drivers
PortOut MotB (PortB, PORT_B_MASK);
// Pin 1 VBAT NET +3V3
@@ -86,7 +88,7 @@
// 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_A_Current (PC_2); // Pin 10
AnalogIn Motor_B_Current (PC_3); // Pin 11
// Pin 12 VSSA/VREF- NET GND
// Pin 13 VDDA/VREF+ NET +3V3
@@ -129,12 +131,15 @@
// 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
+// was DigitalOut T1 (PA_12); // Pin 45
+InterruptIn T1 (PA_12); // Pin 45 now input counting pulses from LMT01 temperature sensor
// Pin 46 SWDIO
// Pin 47 VSS
// Pin 48 VDD
// Pin 49 SWCLK
-DigitalOut T5 (PA_15); // Pin 50
+
+//Was DigitalOut T5 (PA_15); // Pin 50
+DigitalIn T5 (PA_15); // Pin 50 now fwd/rev from remote control box if fitted
InterruptIn MAH1 (PC_10); // Pin 51
InterruptIn MAH2 (PC_11); // Pin 52
InterruptIn MAH3 (PC_12); // Pin 53
@@ -155,29 +160,18 @@
// 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,
- TICKLE_TIMES = 100 ;
-
-/* 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
+int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
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;
+ AtoD_Semaphore = 0;
+int IAm;
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
-double angle = 0.0, angle_step = 0.00005, 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
@@ -185,14 +179,6 @@
// Interrupt Service Routines
-/*uint32_t edgeintcnt = 0;
-void seredgerise () { edgeintcnt++; }
-void seredgefall () { edgeintcnt++; }
-
-void seredgetest () {
- com2.printf ("edgeintcnt = %d\r\n", edgeintcnt);
- com3.printf ("%c", 0x55);
-}*/
/** 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.
@@ -212,21 +198,13 @@
* 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
+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_IN, SERVO_OUT} ;
-
class RControl_In
{ // Read servo style pwm input
Timer t;
@@ -318,7 +296,7 @@
class motor
{
- uint32_t Hall_total, mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
+ uint32_t Hall_total, visible_mode, inner_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;
@@ -332,6 +310,8 @@
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;
+ uint32_t RPM, PPS;
+ double last_V, last_I;
motor () {} ; // Default constructor
motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ;
void set_V_limit (double) ; // Sets max motor voltage
@@ -369,11 +349,8 @@
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;
+ visible_mode = REGENBRAKE;
+ inner_mode = REGENBRAKE;
lut = lutptr;
Hindex[0] = Hindex[1] = read_Halls ();
ppstmp = 0;
@@ -384,8 +361,15 @@
Hall1 = Hall[0];
Hall2 = Hall[1];
Hall3 = Hall[2];
+ PPS = 0;
+ RPM = 0;
+ last_V = last_I = 0.0;
}
+/**
+void motor::direction_set (int dir) {
+Used to set direction according to mode data from eeprom
+*/
void motor::direction_set (int dir) {
if (dir != 0)
dir = FORWARD | REVERSE; // bits used in eor
@@ -429,7 +413,7 @@
p = 0.0;
if (p > 1.0)
p = 1.0;
-// last_pwm = p;
+ last_V = p; // for read by diagnostics
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
@@ -442,6 +426,7 @@
p = 0.0;
if (p > 1.0)
p = 1.0;
+ last_I = p;
a = (int)(p * MAX_PWM_TICKS);
if (a > MAX_PWM_TICKS)
a = MAX_PWM_TICKS;
@@ -465,6 +450,8 @@
Hall_tab_ptr++;
if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
Hall_tab_ptr = 0;
+ PPS = latest_pulses_per_sec;
+ RPM = (latest_pulses_per_sec * 60) / 24;
return latest_pulses_per_sec;
}
@@ -473,15 +460,25 @@
return moving_flag;
}
+/**
+bool motor::set_mode (int m)
+Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
+If this causes change of mode, also sets V and I to zero.
+*/
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 (visible_mode != m) { // Mode change, kill volts and amps to be safe
+ set_V_limit (0.0);
+ set_I_limit (0.0);
+ visible_mode = m;
+ }
if (m == FORWARD || m == REVERSE)
m ^= direction;
- mode = m;
+ inner_mode = m; // idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
return true;
}
@@ -503,11 +500,18 @@
encoder_error_cnt++;
else
angle_cnt += delta_theta;
- *Motor_Port = lut[mode | Hindex[0]];
+ *Motor_Port = lut[inner_mode | Hindex[0]]; // changed mode to inner_mode 27/04/18
Hall_total++;
Hindex[1] = Hindex[0];
}
+ uint32_t temp_sensor_count = 0; // global
+ bool temp_count_in_progress = false;
+
+void temp_sensor_isr () { // got rising edge from LMT01
+ temp_sensor_count++;
+}
+
void MAH_isr ()
{
uint32_t x = 0;
@@ -537,7 +541,7 @@
void motor::motor_set ()
{
Hindex[0] = read_Halls ();
- *Motor_Port = lut[mode | Hindex[0]];
+ *Motor_Port = lut[inner_mode | Hindex[0]];
}
void setVI (double v, double i) {
@@ -555,7 +559,24 @@
MotorB.set_I_limit (i);
}
-void sincostest () {
+void read_RPM (uint32_t * dest) {
+ dest[0] = MotorA.RPM;
+ dest[1] = MotorB.RPM;
+}
+
+void read_PPS (uint32_t * dest) {
+ dest[0] = MotorA.PPS;
+ dest[1] = MotorB.PPS;
+}
+
+void read_last_VI (double * d) { // only for test from cli
+ d[0] = MotorA.last_V;
+ d[1] = MotorA.last_I;
+ d[2] = MotorB.last_V;
+ d[3] = MotorB.last_I;
+}
+
+/*void sincostest () {
sinv = sin(angle); // to set speed and direction of MotorA
cosv = cos(angle); // to set speed and direction of MotorB
Servos[0]->write ((sinv + 1.0) / 2.0);
@@ -577,14 +598,25 @@
cosv = -cosv;
}
MotorB.set_V_limit (0.01 + (cosv / 1.3));
-}
+}*/
+/**
+void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
+Not part of ISR
+*/
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;
+ static uint32_t i = 0, tab_ptr = 0, local_temperature_count = 0;
// sincostest ();
-
+// uint32_t temp_sensor_count = 0; // global
+// bool temp_count_in_progress = false;
+ if (local_temperature_count == temp_sensor_count)
+ temp_count_in_progress = false;
+ else {
+ temp_count_in_progress = true;
+ local_temperature_count = temp_sensor_count;
+ }
if (MotorA.tickleon)
MotorA.high_side_off ();
if (MotorB.tickleon)
@@ -620,7 +652,7 @@
} // end of while (AtoD_Semaphore > 0) {
if (MotorA.tickleon) {
MotorA.tickleon--;
- MotorA.motor_set ();
+ MotorA.motor_set (); // Reactivate any high side switches turned off above
}
if (MotorB.tickleon) {
MotorB.tickleon--;
@@ -635,25 +667,31 @@
*/
double Read_DriverPot ()
{
- return (double) driverpot_reading / 65535.0; // Normalise 0.0 <= control pot <= 1.0
+ return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0
}
double Read_BatteryVolts ()
{
- return (double) volt_reading / 951.0; // divisor fiddled to make voltage reading correct !
+ return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
}
-double read_volts () // A test function
-{
- pc.printf ("pot = %.4f, System Voltage = %.2f\r\n", Read_DriverPot(), Read_BatteryVolts());
- return Read_BatteryVolts();
+void read_supply_vi (double * val) { // called from cli
+ val[0] = MotorA.I.ave;
+ val[1] = MotorB.I.ave;
+ val[2] = Read_BatteryVolts ();
}
-void mode_test (int mode, double val) {
+void mode_set (int mode, double val) { // called from cli to set fw, re, rb, hb
MotorA.set_mode (mode);
MotorB.set_mode (mode);
if (mode == REGENBRAKE) {
-
+ if (val > 1.0)
+ val = 1.0;
+ if (val < 0.0)
+ val = 0.0;
+ val *= 0.9; // set upper limit, this is essential
+ val = sqrt (val); // to linearise effect
+ setVI (val, 1.0);
}
}
@@ -662,7 +700,7 @@
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
+/*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
@@ -671,27 +709,26 @@
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 i = J3;
+// if (i != 0)
+// i = 1;
+// return i | '0';
+ return IAm;
}
int main()
{
int eighth_sec_count = 0;
- uint32_t Apps, Bpps;
MotA = 0; // Output all 0s to Motor drive ports A and B
MotB = 0;
MotPtr[0] = &MotorA; // Pointers to motor class objects
MotPtr[1] = &MotorB;
-
-// tryseredge.rise (&seredgerise);
-// tryseredge.fall (&seredgefall);
+
+ T1.rise (&temp_sensor_isr);
MAH1.rise (& MAH_isr); // Set up interrupt vectors
MAH1.fall (& MAH_isr);
@@ -716,7 +753,6 @@
Servo1_i.mode (PullUp);
Servo2_i.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
@@ -724,39 +760,58 @@
const int TXTBUFSIZ = 36;
char buff[TXTBUFSIZ];
- bool eerom_detected = false;
pc.baud (9600);
com3.baud (1200);
com2.baud (19200);
- pc.printf ("RAM test - ");
- if (check_24LC64() != 0xa0) // searches for i2c devices, returns address of highest found
+ 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");
+ com2.printf ("Check for 24LC64 eeprom FAILED\r\n");
}
- T1 = 0; // As yet unused pins
- T2 = 0;
+ else { // Found 24LC64 memory on I2C
+ bool k;
+// 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");
+// com2.printf("Ram test returned [%s], wr ret'd [%s], rd ret'd [%s]\r\n", buff, j ? "true" : "false", k ? "true" : "false");
+ k = rd_24LC64 (0, buff, 32);
+// if (k)
+// com2.printf ("Good read from eeprom\r\n");
+ if (!k)
+ com2.printf ("Error reading from eeprom\r\n");
+
+ int err = 0;
+ for (int i = 0; i < numofopts; i++) {
+ if ((buff[i] < option_list[i].min) || (buff[i] > option_list[i].max)) {
+ com2.printf ("EEROM error with %s\r\n", option_list[i].t);
+ err++;
+ }
+// else
+// com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
+ }
+ IAm = '0';
+ if (err == 0) {
+ MotorA.direction_set (buff[0]);
+ MotorB.direction_set (buff[1]);
+ IAm = buff[6];
+ }
+ // Alternative ID 1 to 9
+// com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]);
+ }
+// T1 = 0; Now interruptIn counting pulses from LMT01 temperature sensor
+ T2 = 0; // T2, T3, T4 As yet unused pins
T3 = 0;
T4 = 0;
- T5 = 0;
+// T5 = 0; now input from fw/re on remote control box
T6 = 0;
// MotPtr[0]->set_mode (REGENBRAKE);
MotorA.set_mode (REGENBRAKE);
MotorB.set_mode (REGENBRAKE);
- MotorA.set_V_limit (0.9);
- MotorB.set_V_limit (0.9);
- MotorA.set_I_limit (0.5);
- MotorB.set_I_limit (0.5);
+ setVI (0.9, 0.5);
+
Servos[0] = Servos[1] = NULL;
// NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8);
// Only works with unconditional inline code
@@ -788,37 +843,32 @@
break;
}
*/
- MotorA.set_mode (FORWARD);
- MotorB.set_mode (REVERSE);
- MotorA.set_V_limit (0.2);
- MotorB.set_V_limit (0.2);
- MotorA.set_I_limit (0.5);
- MotorB.set_I_limit (0.5);
-
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 ();
+ MotorA.pulses_per_sec (); // Needed to keep table updated to give reading in Hall transitions per second
+ MotorB.pulses_per_sec (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
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
+ WatchDog--;
+ if (WatchDog == 0) { // Deal with WatchDog timer timeout here
+ setVI (0.0, 0.0); // set motor volts and amps to zero
+ com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
+ } // End of dealing with WatchDog timer timeout
+ if (WatchDog < 0)
+ WatchDog = 0;
eighth_sec_count++;
if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts
eighth_sec_count = 0;
- MotorA.current_calc ();
+ MotorA.current_calc (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
MotorB.current_calc ();
-// Apps += Bpps; // to kill compiler warning
-// 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);
- com2.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);
-// com2.printf ("RCI1 pw %d, RCI2 pw %d, 1per %d, 2per %d\r\n", RCI1.pulsewidth(), RCI2.pulsewidth(), RCI1.period(), RCI2.period());
+// com2.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);
}
} // End of if(flag_8Hz)
} // End of main programme loop