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 12:d1d21a2941ef, committed 2019-03-04
- Comitter:
- JonFreeman
- Date:
- Mon Mar 04 17:51:08 2019 +0000
- Parent:
- 11:bfb73f083009
- Commit message:
- STM3 ESC dual motor controller boards. Always 'Work In Progress', working snapshot March 2019
Changed in this revision
--- a/24LC64_eeprom.cpp Sat Jan 19 11:45:01 2019 +0000
+++ b/24LC64_eeprom.cpp Mon Mar 04 17:51:08 2019 +0000
@@ -1,16 +1,129 @@
#include "mbed.h"
-//#include "rtos.h"
+#include "DualBLS.h"
#include "BufferedSerial.h"
extern BufferedSerial pc;
-extern I2C i2c;
+extern error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes.
// 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
const int addr_wr = 0xa0; // set bit 0 for read, clear bit 0 for write
const int ACK = 1;
-bool ack_poll () { // wait short while for any previous memory operation to complete
+struct optpar {
+ int min, max, def; // min, max, default
+ const char * t; // description
+} ;
+struct optpar option_list2[] = {
+ {0, 1, 1, "MotorA direction 0 or 1"},
+ {0, 1, 0, "MotorB direction 0 or 1"},
+ {4, 8, 8, "MotorA poles 4 or 6 or 8"},
+ {4, 8, 8, "MotorB poles 4 or 6 or 8"},
+ {1, 4, 1, "MotorA 0R05 ohm current shunt resistors 1 to 4"},
+ {1, 4, 1, "MotorB 0R05 ohm current shunt resistors 1 to 4"},
+ {0, 1, 0, "Servo1 0 = Not used, 1= Output"},
+ {0, 1, 0, "Servo2 0 = Not used, 1= Output"},
+ {0, 1, 0, "RC Input1 0 = Not used, 1= Output"},
+ {0, 1, 0, "RC Input2 0 = Not used, 1= Output"},
+ {2, 5, 2, "Command source 2= COM2 (Touch Screen), 3= Pot, 4= RC Input1, 5= RC Input2"},
+ {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, // defaults to '0' before eerom setup for first time
+ {10, 250, 60, "Top speed MPH * 10 range 1.0 to 25.0"}, // New Jan 2019 TOP_SPEED
+ {50, 253, 98, "Wheel diameter mm"}, // New 01/06/2018
+ {10, 253, 27, "Motor pinion"}, // New 01/06/2018
+ {50, 253, 85, "Wheel gear"}, // New 01/06/2018
+ {0, 100, 0, "Future 1"},
+ {0, 100, 0, "Future 2"},
+ {0, 100, 0, "Future 3"},
+ {0, 100, 0, "Future 4"},
+ {0, 100, 0, "Future 5"},
+} ;
+
+const int numof_eeprom_options2 = sizeof(option_list2) / sizeof (struct optpar);
+
+
+
+
+/*
+class eeprom_settings {
+ I2C i2c;
+ uint32_t errors;
+ char settings [36];
+ bool rd_24LC64 (int start_addr, char * dest, int length) ;
+ bool wr_24LC64 (int start_addr, char * dest, int length) ;
+ bool set_24LC64_internal_address (int start_addr) ;
+ bool ack_poll () ;
+ public:
+ eeprom_settings (PinName sda, PinName scl); // Constructor
+ char rd (uint32_t) ; // Read one setup char value from private buffer 'settings'
+ bool wr (char, uint32_t) ; // Write one setup char value to private buffer 'settings'
+ bool save () ; // Write 'settings' buffer to EEPROM
+ uint32_t errs () ; // Return errors
+} ;
+*/
+
+bool eeprom_settings::set_defaults () { // Put default settings into EEPROM and local buffer
+ for (int i = 0; i < numof_eeprom_options2; i++)
+ settings[i] = option_list2[i].def; // Load defaults and 'Save Settings'
+ return save ();
+}
+
+uint32_t eeprom_settings::errs () {
+ return errors;
+}
+
+// Use : eeprom_settings (SDA_PIN, SCL_PIN);
+eeprom_settings::eeprom_settings (PinName sda, PinName scl) : i2c(sda, scl) // Constructor
+{
+ errors = 0;
+ for (int i = 0; i < 36; i++)
+ settings[i] = 0;
+ i2c.frequency(400000); // Speed 400000 max.
+ int last_found = 0, q; // Note address bits 3-1 to match addr pins on device
+ for (int i = 0; i < 255; i += 2) { // Search for devices at all possible i2c addresses
+ i2c.start();
+ q = i2c.write(i); // may return error code 2 when no start issued
+ switch (q) {
+ case ACK:
+ pc.printf ("I2C device found at 0x%x\r\n", i);
+ last_found = i;
+ case 2: // Device not seen at this address
+ break;
+ default:
+ pc.printf ("Unknown error %d in check_24LC64\r\n", q);
+ errors |= 512;
+ break;
+ }
+ }
+ i2c.stop();
+ if (errors || last_found != 0xa0) {
+ pc.printf ("Error - EEPROM not seen %d\r\n", errors);
+ errors |= 0xa0;
+ ESC_Error.set (FAULT_EEPROM, errors); // Set FAULT_EEPROM bits if 24LC64 problem
+ }
+ else { // Found 24LC64 memory on I2C. Attempt to load settings from EEPROM
+ errors = 0;
+ if (!rd_24LC64 (0, settings, 32))
+ ESC_Error.set (FAULT_EEPROM, 2); // Set FAULT_EEPROM bit 1 if 24LC64 problem
+ for (int i = 0; i < numof_eeprom_options2; i++) {
+ if ((settings[i] < option_list2[i].min) || (settings[i] > option_list2[i].max)) {
+ pc.printf ("EEROM error with %s\r\n", option_list2[i].t);
+ errors++;
+ }
+ }
+ }
+ ESC_Error.set (FAULT_EEPROM, errors); // sets nothing if 0
+ if (errors > 1) {
+ pc.printf ("Bad settings found at startup. Restoring defaults\r\n");
+ for (int i = 0; i < numof_eeprom_options2; i++)
+ settings[i] = option_list2[i].def; // Load defaults and 'Save Settings'
+ if (!wr_24LC64 (0, settings, 32)) // Save settings
+ pc.printf ("Error saving EEPROM in mode19\r\n");
+ }
+ else // 0 or 1 error max found
+ pc.printf ("At startup, settings errors = %d\r\n", errors);
+} // endof constructor
+
+bool eeprom_settings::ack_poll () { // wait short while for any previous memory operation to complete
const int poll_tries = 40;
int poll_count = 0;
bool i2cfree = false;
@@ -19,14 +132,12 @@
if (i2c.write(addr_wr) == ACK)
i2cfree = true;
else
-// Thread::wait(1); // 1ms
wait_ms (1);
}
-// pc.printf ("ack_poll, count = %d, i2cfree = %s\r\n", poll_count, i2cfree ? "true" : "false");
return i2cfree;
}
-bool set_24LC64_internal_address (int start_addr) {
+bool eeprom_settings::set_24LC64_internal_address (int start_addr) {
if (!ack_poll())
{
pc.printf ("Err in set_24LC64_internal_address, no ACK writing device address byte\r\n");
@@ -44,7 +155,29 @@
return true;
}
-bool wr_24LC64 (int start_addr, char * source, int length) {
+bool eeprom_settings::rd_24LC64 (int start_addr, char * dest, int length) {
+ int acknak = ACK;
+ if(length < 1)
+ return false;
+ if (!set_24LC64_internal_address (start_addr)) {
+ pc.printf ("In rd_24LC64, failed to set_ramaddr\r\n");
+ return false;
+ }
+ i2c.start();
+ if (i2c.write(addr_rd) != ACK) {
+ pc.printf ("Errors in rd_24LC64 sending addr_rd\r\n");
+ return false;
+ }
+ while(length--) {
+ if(length == 0)
+ acknak = 0;
+ *dest++ = i2c.read(acknak);
+ }
+ i2c.stop();
+ return true;
+}
+
+bool eeprom_settings::wr_24LC64 (int start_addr, char * source, int length) {
int err = 0;
if(length < 1 || length > 32) {
pc.printf ("Length out of range %d in wr_24LC64\r\n", length);
@@ -66,46 +199,22 @@
return true;
}
-bool rd_24LC64 (int start_addr, char * dest, int length) {
- int acknak = ACK;
- if(length < 1)
- return false;
- if (!set_24LC64_internal_address (start_addr)) {
- pc.printf ("In rd_24LC64, failed to set_ramaddr\r\n");
- return false;
+char eeprom_settings::rd (uint32_t i) { // Read one setup char value from private buffer 'settings'
+ if (i > 31) {
+ pc.printf ("ERROR Attempt to read setting %d\r\n");
+ return 0;
}
- i2c.start();
- if (i2c.write(addr_rd) != ACK) {
- pc.printf ("Errors in rd_24LC64 sending addr_rd\r\n");
+ return settings[i];
+}
+
+bool eeprom_settings::wr (char c, uint32_t i) { // Read one setup char value from private buffer 'settings'
+ if (i > 31)
return false;
- }
- while(length--) {
- if(length == 0)
- acknak = 0;
- *dest++ = i2c.read(acknak);
- }
- i2c.stop();
+ settings[i] = c;
return true;
}
-int check_24LC64 () { // Call from near top of main() to init i2c bus
-// i2c.frequency(400000); // Speed 400000 max.
- i2c.frequency(400000); // Speed 400000 max.
- int last_found = 0, q; // Note address bits 3-1 to match addr pins on device
- for (int i = 0; i < 255; i += 2) { // Search for devices at all possible i2c addresses
- i2c.start();
- q = i2c.write(i); // may return error code 2 when no start issued
- switch (q) {
- case ACK:
- pc.printf ("I2C device found at 0x%x\r\n", i);
- last_found = i;
- case 2: // Device not seen at this address
- break;
- default:
- pc.printf ("Unknown error %d in check_24LC64\r\n", q);
- break;
- }
- }
- i2c.stop();
- return last_found;
+bool eeprom_settings::save () { // Write 'settings' buffer to EEPROM
+ return wr_24LC64 (0, settings, 32);
}
+
--- a/DualBLS.h Sat Jan 19 11:45:01 2019 +0000
+++ b/DualBLS.h Mon Mar 04 17:51:08 2019 +0000
@@ -3,20 +3,24 @@
#ifndef MBED_DUALBLS_H
#define MBED_DUALBLS_H
-const int HANDBRAKE = 0,
- FORWARD = 8,
- REVERSE = 16,
- REGENBRAKE = 24;
+//#define USING_DC_MOTORS // Uncomment this to play with Dinosaur DC motors
-const int TIMEOUT_SECONDS = 30;
+#include "BufferedSerial.h"
+const int MOTOR_HANDBRAKE = 0,
+ MOTOR_FORWARD = 8,
+ MOTOR_REVERSE = 16,
+ MOTOR_REGENBRAKE = 24;
+
+const int TIMEOUT_SECONDS = 2;
/* Please Do Not Alter these */
-const int PWM_PRESECALER_DEFAULT = 5,
+const int PWM_PRESECALER_DEFAULT = 2,
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
+// CURRENT_SAMPLES_AVERAGED = 100, // Current is spikey. Reading smoothed by using average of this many latest current readings
PWM_HZ = 15000, // chosen to be above cutoff frequency of average human ear
+// PWM_HZ = 8000, // chosen to be above cutoff frequency of average human ear - clearly audible annoying noise
MAX_PWM_TICKS = (SystemCoreClock / (PWM_HZ * PWM_PRESECALER_DEFAULT)),
TICKLE_TIMES = 100 ,
WATCHDOG_RELOAD = (TIMEOUT_SECONDS * 8); // WatchDog counter ticked down in 8Hz loop
@@ -27,36 +31,95 @@
enum {COM_SOURCES, COM1, COM2, HAND, RC_IN1, RC_IN2,THEEND} ;
-//enum {MOTADIR, MOTBDIR, GANG, SVO1, SVO2, COMM_SRC, ID, WHEELDIA, MOTPIN, WHEELGEAR} ; // Identical in TS and DualBLS
-enum {MOTADIR, MOTBDIR, GANG, SVO1, SVO2, COMM_SRC, ID, WHEELDIA, MOTPIN, WHEELGEAR, BOGHUNWAT, FUT1, FUT2, FUT3, FUT4, FUT5} ; // Identical in TS and DualBLS
-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
- {50, 253, 98, "Wheel diameter mm"}, // New 01/06/2018
- {10, 253, 27, "Motor pinion"}, // New 01/06/2018
- {50, 253, 85, "Wheel gear"}, // New 01/06/2018
- {1, 20, 4, "Bogie power closest hundreds of Watt"}, // New 22/06/2018
- {0, 100, 0, "Future 1"},
- {0, 100, 0, "Future 2"},
- {0, 100, 0, "Future 3"},
- {0, 100, 0, "Future 4"},
- {0, 100, 0, "Future 5"},
-} ;
-const int numof_eeprom_options = sizeof(option_list) / sizeof (struct optpar);
+enum {MOTADIR, MOTBDIR, MOTAPOLES, MOTBPOLES, ISHUNTA, ISHUNTB, SVO1, SVO2, RCIN1, RCIN2,
+ COMM_SRC, BOARD_ID, TOP_SPEED, WHEELDIA, MOTPIN, WHEELGEAR,
+ FUT1, FUT2, FUT3, FUT4, FUT5} ; //
-struct single_bogie_options {
- char motoradir, motorbdir, gang, svo1, svo2, comm_src, id, wheeldia, motpin, wheelgear, spare;
+enum {
+ FAULT_0,
+ FAULT_EEPROM,
+ FAULT_BOARD_ID,
+ FAULT_COM_LINE_LEN,
+ FAULT_COM_LINE_NOMATCH,
+ FAULT_COM_LINE_LEN_PC,
+ FAULT_COM_LINE_LEN_TS,
+ FAULT_COM_LINE_NOMATCH_PC,
+ FAULT_COM_LINE_NOMATCH_TS,
+ FAULT_UNRECOGNISED_STATE,
+ FAULT_MAX,
+ NUMOF_REPORTABLE_TS_ERRORS
+ } ;
+
+class error_handling_Jan_2019
+{
+ int32_t ESC_fault[NUMOF_REPORTABLE_TS_ERRORS] ; // Some number of reportable error codes, accessible through set and read members
+ public:
+ error_handling_Jan_2019 () { // default constructor
+ for (int i = 0; i < (sizeof(ESC_fault) / sizeof(int32_t)); i++)
+ ESC_fault[i] = 0;
+ }
+ void set (uint32_t, int32_t) ;
+ void clr (uint32_t) ;
+ uint32_t read (uint32_t) ;
+ bool all_good () ;
+ void report_any (bool) ; // retain ? true or false
} ;
-//const double SERVOIN_PWR_BENDER = 1.5; // Used to change servo_in stick at centre position to match pot approx 1/3 braking 2/3 driving
+enum {SOURCE_PC, SOURCE_TS} ;
+const int BROADCAST = '\r';
+const int MAX_PARAMS = 20;
+const int MAX_CMD_LEN = 220;
+
+struct parameters {
+ struct kb_command const * command_list;
+ BufferedSerial * com; // pc or com2
+ int32_t position_in_list, numof_dbls, target_unit, source, numof_menu_items;
+ double dbl[MAX_PARAMS];
+ bool respond, resp_always;
+} ;
+
+class cli_2019 {
+ struct kb_command const * commandlist ;
+ int clindex;
+ char cmdline[MAX_CMD_LEN + 8];
+ char * cmdline_ptr;
+ parameters a ;
+public:
+ cli_2019 (BufferedSerial * comport, kb_command const * list, int list_len, int source) {
+ a.com = comport ;
+ a.command_list = commandlist = list ;
+ a.numof_menu_items = list_len ;
+ a.source = source ;
+ cmdline_ptr = cmdline;
+ clindex = 0;
+ if (source == SOURCE_PC)
+ a.resp_always = true;
+ else
+ a.resp_always = false;
+ } ;
+ void core () ;
+ void test () ;
+} ;
+
+class eeprom_settings {
+ I2C i2c;
+ uint32_t errors;
+ char settings [36];
+ bool rd_24LC64 (int start_addr, char * dest, int length) ;
+ bool wr_24LC64 (int start_addr, char * dest, int length) ;
+ bool set_24LC64_internal_address (int start_addr) ;
+ bool ack_poll () ;
+ public:
+ eeprom_settings (PinName sda, PinName scl); // Constructor
+ char rd (uint32_t) ; // Read one setup char value from private buffer 'settings'
+ bool wr (char, uint32_t) ; // Write one setup char value to private buffer 'settings'
+ bool save () ; // Write 'settings' buffer to EEPROM
+ bool set_defaults (); // Put default settings into EEPROM and local buffer
+ uint32_t errs () ; // Return errors
+} ;
+
+
+
+
#endif
--- a/F401RE.h Sat Jan 19 11:45:01 2019 +0000 +++ b/F401RE.h Mon Mar 04 17:51:08 2019 +0000 @@ -90,7 +90,7 @@ // Pin 14 Port_A AUL // Pin 15 Port_A AUH // Pins 16, 17 BufferedSerial pc -BufferedSerial pc (PA_2, PA_3, 512, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead +BufferedSerial pc (PA_2, PA_3, 2048, 4, NULL); // Pins 16, 17 tx, rx to pc via usb lead // Pin 18 VSS NET GND // Pin 19 VDD NET +3V3 // Pin 20 Port_A AWL @@ -147,12 +147,9 @@ //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 -#define _MAH1 PC_10 -#define _MAH2 PC_11 -#define _MAH3 PC_12 +#define _MAH1 PC_10 // Pin 51 +#define _MAH2 PC_11 // Pin 52 +#define _MAH3 PC_12 // Pin 53 //InterruptIn MBH1 (PD_2); // Pin 54 #define _MBH1 PD_2 DigitalOut T6 (PB_3); // Pin 55 @@ -161,7 +158,9 @@ //FastPWM B_MAX_V_PWM (PB_4, PWM_PRESECALER_DEFAULT), // Pin 56 pwm3/3 // B_MAX_I_PWM (PB_5, PWM_PRESECALER_DEFAULT); // pin 57, prescaler value pwm3/4 -I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom +//I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom +#define SDA_PIN PB_7 +#define SCL_PIN PB_6 // Pin 60 BOOT0 // Servo pins, 2 off. Configured as Input to read radio control receiver
--- a/Radio_Control_In.cpp Sat Jan 19 11:45:01 2019 +0000
+++ b/Radio_Control_In.cpp Mon Mar 04 17:51:08 2019 +0000
@@ -39,6 +39,12 @@
return period_us;
}
+void RControl_In::reset ()
+{
+ pulse_width_us = period_us = pulse_count = 0;
+ t.reset ();
+}
+
bool RControl_In::validate_rx ()
{ // Tests for pulse width and repetition rates being believable
return !((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200));
--- a/Radio_Control_In.h Sat Jan 19 11:45:01 2019 +0000
+++ b/Radio_Control_In.h Mon Mar 04 17:51:08 2019 +0000
@@ -27,6 +27,7 @@
lost_chan_return_value = 0.0;
} ;
bool validate_rx () ; // Informs whether signal being rx'd
+ void reset () ;
void set_lost_chan_return_value (double) ; // set what 'normalised' returns when no signal
uint32_t pulsecount () ; // will count up at frame rate when radio control all working well
uint32_t pulsewidth () ;
--- a/Servo.lib Sat Jan 19 11:45:01 2019 +0000 +++ b/Servo.lib Mon Mar 04 17:51:08 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07 +https://os.mbed.com/users/simon/code/Servo/#4e95f1bbbf51
--- a/brushless_motor.cpp Sat Jan 19 11:45:01 2019 +0000
+++ b/brushless_motor.cpp Mon Mar 04 17:51:08 2019 +0000
@@ -8,14 +8,17 @@
#include "Servo.h"
#include "brushless_motor.h"
+extern eeprom_settings mode ;
+extern double rpm2mph ;
+extern BufferedSerial pc; // The two com ports used. There is also an unused com port, com3 setup @ 1200 baud
brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi,
- const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask)
+ const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask, uint32_t rnum)
: Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask) // Constructor
{
// Constructor
OP = 0;
- H1.mode (PullUp);
+ H1.mode (PullUp); // PullUp resistors enabled on all Hall sensor input pins
H2.mode (PullUp);
H3.mode (PullUp);
H1.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
@@ -25,34 +28,65 @@
H3.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
H3.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
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;
- Hall_tab_ptr = 0;
+ Hall_previous = 0;
maxV.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
maxI.period_ticks (MAX_PWM_TICKS + 1);
- maxV.pulsewidth_ticks (MAX_PWM_TICKS / 20);
- maxI.pulsewidth_ticks (MAX_PWM_TICKS / 30);
- visible_mode = REGENBRAKE;
- inner_mode = REGENBRAKE;
- lut = lutptr;
+ maxV.pulsewidth_ticks (MAX_PWM_TICKS - 20); // Motor voltage pwm is inverted, see MCP1630 data
+ maxI.pulsewidth_ticks (MAX_PWM_TICKS / 30); // Motor current pwm is not inverted. Initial values for scope hanging test
+ visible_mode = MOTOR_REGENBRAKE;
+ inner_mode = MOTOR_REGENBRAKE;
+ lut = lutptr; // Pointer to motor energisation bit pattern table
+ current_sense_rs_offset = rnum; // This is position in mode.rd(current_sense_rs_offset)
Hall_index[0] = Hall_index[1] = read_Halls ();
- ppstmp = 0;
- cs_ptr = 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
- PPS = 0;
- RPM = 0;
last_V = last_I = 0.0;
+ Idbl = 0.0;
+ numof_current_sense_rs = 1.0;
+ RPM_filter = 0.0;
+ dv_by_dt = 0.0;
+ s[1] = 0.25;
+ s[2] = 9.0;
+ s[3] = 0.4;
+ s[4] = 0.2;
+ dRPM = 0.0;
+ V_clamp = 1.0 ; // Used to limit top speed
+ motor_poles = 8; // Default to 8 pole motor
+#ifdef USING_DC_MOTORS
dc_motor = (Hall_index[0] == 7) ? true : false ;
+#endif
}
-void brushless_motor::sniff_current () {
- current_samples[cs_ptr++] = Motor_I.read_u16 (); //
- if (cs_ptr >= CURRENT_SAMPLES_AVERAGED)
- cs_ptr = 0;
+/**
+* void brushless_motor::sniff_current () { // Initiate ADC current reading
+* This to be called in response to ticker timebase interrupt.
+* As designed, called at 200 micro second intervals (Feb 2019)
+* Updates double I.dbl current measured in milliamps
+* Reading not used elsewhere in this code but made available through command line for external controller
+*/
+void brushless_motor::sniff_current () { // Initiate ADC current reading
+ const double sampweight = 0.01 ; /// (double)CURRENT_SAMPLES_AVERAGED ;
+ const double shrinkby = 1.0 - sampweight;
+ uint16_t samp = Motor_I.read_u16 (); // CHECK but thought to be called once per 200us for each motor - CORRECT Feb 2019
+ double dbls = ((double)samp) * numof_current_sense_rs / 6.0; // reading in mA
+ Idbl *= shrinkby; // Jan 2019 New recursive low pass filter
+ Idbl += dbls * sampweight; // Current reading available, however not certain this is of any real use
+}
+
+bool brushless_motor::poles (int p) { // Jan 2019 max_rom no longer used. target_speed is preferred
+ if (!max_rpm) { // Not been set since powerup
+ max_rpm = (uint32_t) (((double)mode.rd(TOP_SPEED) / rpm2mph) / 10.0) ;
+ target_speed = (double)mode.rd(TOP_SPEED) / 10.0;
+ numof_current_sense_rs = (double)mode.rd(current_sense_rs_offset);
+ pc.printf ("max_rpm=%d, tp speed=%.1f, rpm2mph=%.6f\r\n", max_rpm, target_speed, rpm2mph);
+ }
+ if (p == 4 || p == 6 || p == 8) {
+ motor_poles = p;
+ return true;
+ }
+ return false;
}
void brushless_motor::Hall_change () {
@@ -80,127 +114,163 @@
Hall_index[1] = Hall_index[0];
}
-bool brushless_motor::motor_is_brushless ()
-{
- /* int x = read_Halls ();
- if (x < 1 || x > 6)
- return false;
- return true;
- */
- return !dc_motor;
-}
-
/**
-void brushless_motor::direction_set (int dir) {
-Used to set direction according to mode data from eeprom
+* void brushless_motor::direction_set (int dir) {
+* Used to set direction according to mode data from eeprom
*/
void brushless_motor::direction_set (int dir)
{
- if (dir != 0)
- dir = FORWARD | REVERSE; // bits used in eor
- direction = dir;
+ direction = (dir != 0) ? MOTOR_FORWARD | MOTOR_REVERSE : 0; // bits used in eor, FORWARD = 1<<3, REVERSE = 1<<4
}
int brushless_motor::read_Halls ()
{
int x = 0;
- if (H1 != 0) x |= 1;
- if (H2 != 0) x |= 2;
- if (H3 != 0) x |= 4;
+ if (H1) x |= 1;
+ if (H2) x |= 2;
+ if (H3) x |= 4;
return x;
}
-void brushless_motor::high_side_off ()
+void brushless_motor::high_side_off () // Jan 2019 Only ever called from main when high side gate drive charge might need pumping up
{
-// uint16_t p = *Motor_Port;
uint16_t p = OP;
p &= lut[32]; // KEEP_L_MASK_A or B
-// *Motor_Port = p;
OP = p;
}
-
+/*
void brushless_motor::low_side_on ()
{
-// uint16_t p = *Motor_Port;
-// p &= lut[31]; // KEEP_L_MASK_A or B
-// *Motor_Port = lut[31];
- OP = lut[31];
+ maxV.pulsewidth_ticks (1);
+ OP = lut[31]; // KEEP_L_MASK_A or B
}
+*/
-void brushless_motor::current_calc ()
+void brushless_motor::set_speed (double p) // Sets target_speed
{
- I.min = 0x0fffffff; // samples are 16 bit
- I.max = 0;
- I.ave = 0;
- uint16_t sample;
- for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
- sample = current_samples[i];
- I.ave += sample;
- if (I.min > sample)
- I.min = sample;
- if (I.max < sample)
- I.max = sample;
- }
- I.ave /= CURRENT_SAMPLES_AVERAGED;
+ target_speed = p;
}
-void brushless_motor::raw_V_pwm (int v)
-{
- if (v < 1) v = 1;
- if (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
- maxV.pulsewidth_ticks (v);
-}
-
-void brushless_motor::set_V_limit (double p) // Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
+/**
+* void brushless_motor::set_V_limit (double p) // Sets max motor voltage.
+*
+* Set motor voltage limit from zero (p=0.0) to max link voltage (p=1.0)
+*/
+void brushless_motor::set_V_limit (double p) // Sets max motor voltage.
{
if (p < 0.0)
p = 0.0;
if (p > 1.0)
p = 1.0;
- last_V = p; // for read by diagnostics
- p *= 0.95; // need limit, ffi see MCP1630 data
+ last_V = p; // Retains last voltage limit demanded by driver
+
+ if ((V_clamp < last_V) && (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE)) // Jan 2019 limit top speed when driving
+ p = V_clamp; // If motor runnable, set voltage limit to lower of last_V and V_clamp
+
+ p *= 0.95; // need limit, ffi see MCP1630 data
p = 1.0 - p; // because pwm is wrong way up
maxV.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
}
+/**void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
+*
+* Set motor current limit from zero (p=0.0) to max determined by current sense resistors (p=1.0)
+* Value sent to pwm with RC integrator acting as AnalogOut.
+* pwm capable of 0.0 <= V_out <= 3.3. This feeds MCP1630 V_Ref, range 0 to 2.7v
+* Therefore (2.7/3.3) = 0.82 factor included.
+* Jan 2019 - As yet uncalibrated, so let's have a go at working it out!
+* Voltage ax current sense resistors amplified by op-amp with gain 5.7 (see EasyPC file 'BrushlessSTM3.sch', U6, R63, R64)
+* This then put through pot divider (10k with 4k7 to ground, gain 0.32) reducing overall gain to 1.8 (accurate enough)
+* This connects to MCP1630 CS (current sense) pin which works over the voltage range 0.0 to 0.9v
+* Therefore 0.5v across current sense resistor is sufficient to turn driver off.
+* 0.5v across 0.05 ohm gives 10A per current sense resistor fitted.
+* ** NOTE ** This is fast acting cycle by cycle current limit, the 10A figure is therefore peak T_on current.
+*
+* Current flows through current sense reaistor when one high side and one low side switch are on, expect a rising ramp due to motor inductance.
+* When either switch is off, inductor current continues to flow but not through current sense resistors, through a parasitic diode instead.
+* Thus T_on current is measured, T_off current is not measured
+* This means current reading should approximate to current taken from the supply. Motor average current may be considerably higher.
+* During REGEN_BRAKING, current flows the 'wrong' way through sense resistors and can not be measured.
+*
+* Board designed to have 1, 2, 3 or 4 0R05 current sense resistors per motor for 10A, 20A, 30A or 40A peak motor currents
+*/
void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
{
- int a;
+ const uint32_t MPR = ((MAX_PWM_TICKS * 9) / 11); // Scales 3.3v pwm DAC output to 2.7v V_Ref input
if (p < 0.0)
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;
- if (a < 0)
- a = 0;
- maxI.pulsewidth_ticks (a); // PWM
+ last_I = p; // Retains last current limit demanded by driver
+ maxI.pulsewidth_ticks ((uint32_t)(p * MPR)); // PWM
}
-uint32_t brushless_motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
+
+/**
+* void brushless_motor::speed_monitor_and_control () // ** CALL THIS 32 TIMES PER SECOND **
+* Call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep RPM and MPH figures correct
+* Tracks total transitions on Hall sensor inputs to determine speed.
+* Sets variables double dRPM of motor RPM, and double dMPH miles per hour
+*
+* Speed control - double target_speed as reference input. *
+* ** This is where any speed limit gets applied **
+* Motor voltage reduced when at or over speed. Does NOT apply any braking
+* Scope for further improvement of control algorithm - crude implementation of PID with no I
+*/
+void brushless_motor::speed_monitor_and_control () // 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
+#ifdef USING_DC_MOTORS
if (dc_motor)
return 0;
- if (ppstmp == Hall_total) {
-// if (dc_motor || ppstmp == Hall_total) {
- moving_flag = false; // Zero Hall transitions since previous call - motor not moving
- tickleon = TICKLE_TIMES;
- } else {
- moving_flag = true;
- ppstmp = Hall_total;
+#endif
+// Feb 2019 - coefficients currently values in ram to allow for tweaking via command line. Will be 'const' once settled upon.
+// const double samp_scale = 0.35; // Tweak this value only to tune filter
+ double samp_scale = s[1]; // Tweak this value only to tune filter
+ double shrink_by = 1.0 - samp_scale;
+// const double dv_scale = 0.15;
+ double dv_scale = s[3];
+ double dv_shrink = 1.0 - dv_scale;
+ double speed_error, d, t;
+ uint32_t Hall_tot_copy = Hall_total; // Copy value for use throughout function as may get changed at any instant during exec !
+ moving_flag = true;
+ if (Hall_previous == Hall_tot_copy) { // Test for motor turning or not
+ moving_flag = false; // Zero Hall transitions since previous call - motor not moving
+ tickleon = TICKLE_TIMES; // May need to tickle some charge into high side switch bootstrap supplies
+ }
+ d = (double) ((Hall_tot_copy - Hall_previous) *640); // (Motor Hall sensor transitions in previous 31250us) * 640
+ d /= motor_poles; // d now true but lumpy 'RPM' during most recent 31250us corrected for number of motor poles
+ t = RPM_filter; // Value from last time around
+ RPM_filter *= shrink_by;
+ RPM_filter += (d * samp_scale); // filtered RPM
+ // RPM_filter[n] = shrink_by RPM_filter[n - 1] + samp_scale x[n]
+ t -= RPM_filter; // more like minus dv/dt
+ dv_by_dt *= dv_shrink;
+ dv_by_dt += (t * dv_scale); // filtered rate of change, the 'D' Differential contribution to PID control
+ dRPM += RPM_filter;
+ dRPM /= 2.0;
+ dMPH = RPM_filter * rpm2mph; // Completed updates of RPM and MPH
+
+ if (inner_mode == MOTOR_FORWARD || inner_mode == MOTOR_REVERSE) { // Speed control only makes sense when motor runnable
+ speed_error = (target_speed - dMPH) / 1000.0; // 'P' Proportional contribution to PID control
+ d = V_clamp + (speed_error * s[2]) + ((dv_by_dt / 1000.0) * s[4]); // Apply P+I+D (with I=0) control
+ if (d > 1.0) d = 1.0;
+ if (d < 0.0) d = 0.0;
+ V_clamp = d;
+ if (V_clamp < last_V) // Jan 2019 limit top speed when driving
+ {
+ d *= 0.95; // need limit, ffi see MCP1630 data
+ d = 1.0 - d; // because pwm is wrong way up
+ maxV.pulsewidth_ticks ((int)(d * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
+ }
}
- 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;
- PPS = latest_pulses_per_sec;
- RPM = (latest_pulses_per_sec * 60) / 24;
- return latest_pulses_per_sec;
+/* temp_tick++;
+ if (temp_tick > 35) { // one and a bit second
+ temp_tick = 0;
+ pc.printf ("RPM %.0f, %.3f, %.3f, %.2f, dv/dt%.3f\r\n", dRPM, RPM_filter, d, dMPH, dv_by_dt);
+ }
+*/
+ Hall_previous = Hall_tot_copy;
}
bool brushless_motor::is_moving ()
@@ -215,7 +285,7 @@
*/
bool brushless_motor::set_mode (int m)
{
- if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
+ if ((m != MOTOR_HANDBRAKE) && (m != MOTOR_FORWARD) && (m != MOTOR_REVERSE) && (m != MOTOR_REGENBRAKE)) {
// pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
return false;
}
@@ -224,16 +294,15 @@
set_I_limit (0.0);
visible_mode = m;
}
- if (m == FORWARD || m == REVERSE)
+ if (m == MOTOR_FORWARD || m == MOTOR_REVERSE)
m ^= direction;
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;
}
-
-void brushless_motor::motor_set ()
+void brushless_motor::motor_set () // Energise Port with data determined by Hall sensors
{
Hall_index[0] = read_Halls ();
-// *Motor_Port = lut[inner_mode | Hindex[0]];
OP = lut[inner_mode | Hall_index[0]];
}
+
--- a/brushless_motor.h Sat Jan 19 11:45:01 2019 +0000
+++ b/brushless_motor.h Mon Mar 04 17:51:08 2019 +0000
@@ -7,49 +7,53 @@
#include "mbed.h"
#include "DualBLS.h"
+#include "FastPWM.h"
class brushless_motor
{
int32_t angle_cnt;
- uint32_t Hall_index[2], encoder_error_cnt;
- 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;
- uint16_t ttabl[34];
- FastPWM maxV;
- FastPWM maxI;
- InterruptIn H1;
- InterruptIn H2;
- InterruptIn H3;
- PortOut OP;
+ uint32_t Hall_index[2], encoder_error_cnt, motor_poles, current_sense_rs_offset;
+ uint32_t Hall_total, // Incremented on every Hall sensor transition
+ Hall_previous,
+ visible_mode,
+ inner_mode;
+ uint32_t direction;
+ int temp_tick;
+ double RPM_filter, dv_by_dt;
+ double target_speed;
+ bool moving_flag;
+ const uint16_t * lut;
AnalogIn Motor_I;
- void Hall_change () ;
- int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs
+ FastPWM maxV, maxI;
+ InterruptIn H1, H2, H3; // Inputs for motor Hall sensors
+ PortOut OP;
+ void Hall_change () ;
+ int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs
+ uint32_t max_rpm ;
+ double V_clamp ; // Used to limit top speed
+ double numof_current_sense_rs;
public:
+#ifdef USING_DC_MOTORS
bool dc_motor;
- struct currents {
- uint32_t max, min, ave;
- } I;
- uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
- uint32_t cs_ptr;
- uint32_t RPM, PPS, tickleon;
+#endif
+ uint32_t tickleon;
+ double Idbl;
double last_V, last_I;
+ double dRPM, dMPH, s[8];
// brushless_motor () {} ; // can not use this with exotic elements PortOut, FastPWM etc
- brushless_motor (PinName iadc, PinName pwv, PinName pwi, const uint16_t *, PinName h1, PinName h2, PinName h3, PortName, int) ; // Constructor
+ brushless_motor (PinName iadc, PinName pwv, PinName pwi, const uint16_t *, PinName h1, PinName h2, PinName h3, PortName, int, uint32_t) ; // Constructor
+ bool poles (int) ; // Set number of motor poles - 4, 6, or 8
+ void set_speed (double) ; // Sets target_speed
void set_V_limit (double) ; // Sets max motor voltage
void set_I_limit (double) ; // Sets max motor current
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
+ void direction_set (int) ; // sets 'direction' with bit pattern to eor with MOTOR_FORWARD or MOTOR_REVERSE in set_mode
+ bool set_mode (int); // sets mode to MOTOR_HANDBRAKE, MOTOR_FORWARD, MOTOR_REVERSE or MOTOR_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
- bool motor_is_brushless ();
+ void speed_monitor_and_control () ; // call this once per main loop pass (32Hz) to keep count = edges per sec
void high_side_off () ;
- void low_side_on () ;
- void raw_V_pwm (int);
- void sniff_current () ;
+// void low_side_on () ;
+ void sniff_current () ; // Call this every 200us to update Idbl
} ; //MotorA, MotorB, or even Motor[2];
#endif
--- a/cli_BLS_nortos.cpp Sat Jan 19 11:45:01 2019 +0000
+++ b/cli_BLS_nortos.cpp Mon Mar 04 17:51:08 2019 +0000
@@ -1,300 +1,447 @@
-// DualBLS2018_06
+/**
+* Code in this file : -
+*
+* STM3_ESC board uses two serial ports.
+* One (use pc.printf etc) provides 9600 baud comms to a pc or other terminal. Essential for test and setup, not used in everyday use.
+*
+* com2 provides 19200 baud comms via opto-isolators to Touch Screen Controller (see "Brute_TS_Controller_2018_11"). Opto isolation allows
+* for several boards to parallel up on this port, each STM3_ESC board having a unique single byte ID char in range '0' to '9'.
+* This enables Touch Screen controller to address ESC boards individually e.g. for requesting speed, RPM etc
+* while also allowing broadcast of commands not requiring responses
+*
+* Code implements a Command Line Interpreter (see class cli_2019)
+* Two instantiations of class cli_2019 are used, 'pcc' for pc comms and 'tsc' for touch screen comms
+* These read incoming commands and execute code functions accordingly. These functions are all of type
+* void func (struct parameters &) ;
+* This allows any command line parameters to pass to 'func'
+*/
+// Brushless_STM3_Ctrl_2018_11
#include "mbed.h"
#include "BufferedSerial.h"
-#include "FastPWM.h"
#include "DualBLS.h"
#include "brushless_motor.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'
+extern eeprom_settings mode ;
+extern error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes.
extern int WatchDog;
extern bool WatchDogEnable;
-extern char mode_bytes[];
-
-extern brushless_motor MotorA, MotorB;
+extern double rpm2mph ;
-const int BROADCAST = '\r';
-const int MAX_PARAMS = 20;
-struct parameters {
- struct kb_command const * command_list;
- BufferedSerial * com; // pc or com2
- char cmd_line[120];
- char * cmd_line_ptr;
- int32_t position_in_list, numof_dbls, target_unit, numof_menu_items, cl_index, gp_i;
- double dbl[MAX_PARAMS];
- bool respond, resp_always;
-} ;
+extern brushless_motor MotorA, MotorB; // Controlling two motors together or individually
+extern char const_version_string[] ;
-struct parameters pccom, lococom;
-// WithOUT RTOS
-extern BufferedSerial com2, pc;
-extern void send_test () ;
-extern void setVI (double v, double i) ;
-extern void setV (double v) ;
-extern void setI (double i) ;
-//extern void last_VI (double * val) ; // only for test from cli
-
-//BufferedSerial * com;
+extern BufferedSerial com2, pc; // The two com ports used. There is also an unused com port, com3 setup @ 1200 baud
+extern void setVI (double v, double i) ; // Set motor voltage limit and current limit
extern double Read_DriverPot ();
extern double Read_BatteryVolts ();
-void pot_cmd (struct parameters & a)
+extern void mode_set_both_motors (int mode, double val) ; // called from cli to set fw, re, rb, hb
+extern void rcin_report () ;
+
+// All void func (struct parameters &) ; functions addressed by command line interpreter are together below here
+
+/**
+void ver_cmd (struct parameters & a)
+ Responds YES, causes action NO
+ PC or TS able to read software / firmware / hardware version string
+*/
+void ver_cmd (struct parameters & a)
{
- pc.printf ("Driver's pot %.3f\r\n", Read_DriverPot ());
+ if (a.source == SOURCE_PC)
+ pc.printf ("Version %s\r\n", const_version_string);
+ else {
+ if (a.source == SOURCE_TS)
+ if (a.respond) // Only respond if this board addressed
+ a.com->printf ("%s\r", const_version_string);
+ else
+ pc.printf ("Crap source %d in ver_cmd\r\n", a.source);
+ }
}
+/**
+void pot_cmd (struct parameters & a)
+ Responds YES, causes action NO
+ pc reads DriverPot. No sense in TS reading as STM3_ESC uses either/or TS, DriverPot
+*/
+void pot_cmd (struct parameters & a)
+{ if (a.source == SOURCE_PC)
+ pc.printf ("Driver's pot %.3f\r\n", Read_DriverPot ());
+ else
+ pc.printf ("Wrong use of pot_cmd\r\n");
+}
+
+/**
+* Do nothing command, but does report board ID code '0' to '9'
+*/
void null_cmd (struct parameters & a)
{
if (a.respond)
- a.com->printf ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
-}
-
-// {"wden", "enable watchdog if modes allow", wden_lococmd},
-// {"wddi", "disable watchdog always", wddi_lococmd},
-
-void wden_lococmd (struct parameters & a)
-{
- if (mode_bytes[COMM_SRC] != 3) // When not hand pot control, allow watchdog enable
- WatchDogEnable = true;
-}
-void wden_pccmd (struct parameters & a)
-{
- wden_lococmd (a);
- a.com->printf ("Watchdog %sabled\r\n", WatchDogEnable ? "En":"Dis");
+ a.com->printf ("At null_cmd, board ID %c\r\n", mode.rd(BOARD_ID));
}
-void wddi_lococmd (struct parameters & a)
-{
- WatchDogEnable = false;
-}
-void wddi_pccmd (struct parameters & a)
-{
- wddi_lococmd (a);
- a.com->printf ("Watchdog %sabled\r\n", WatchDogEnable ? "En":"Dis");
-}
-
-extern void report_motor_types () ;
+#ifdef USING_DC_MOTORS
+/**
+void mt_cmd (struct parameters & a)
+ PC Only
+ Responds YES, causes action NO
+ report_motor_types () // Reports 'Brushless' if Hall inputs read 1 to 6, 'DC' if no Hall sensors connected, therefore DC motor assumed
+*/
void mt_cmd (struct parameters & a)
{
- report_motor_types ();
-// if (a.respond)
-// a.com->printf ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
+ if (a.source == SOURCE_PC)
+ pc.printf ("Mot A is %s, Mot B is %s\r\n", MotorA.dc_motor ? "DC":"Brushless", MotorB.dc_motor ? "DC":"Brushless");
+ else
+ pc.printf ("Wrong use of mt_cmd\r\n");
+}
+#endif
+
+/**
+* void rdi_cmd (struct parameters & a) // read motor currents (uint32_t) and BatteryVolts (double)
+*/
+void rdi_cmd (struct parameters & a) // read motor currents (uint32_t) and BatteryVolts (double)
+{ // Voltage reading true volts, currents only useful as relative values
+ if (a.respond)
+// a.com->printf ("rdi%d %d %.1f\r%s", MotorA.I.ave, MotorB.I.ave, Read_BatteryVolts (), a.source == SOURCE_PC ? "\n" : "");
+ a.com->printf ("rdi%.1f %.1f %.1f\r%s", MotorA.Idbl, MotorB.Idbl, Read_BatteryVolts (), a.source == SOURCE_PC ? "\n" : "");
+ // Format good to be unpicked by cli in touch screen controller
}
-extern void mode_set_both_motors (int mode, double val) ; // called from cli to set fw, re, rb, hb
-
-void rdi_cmd (struct parameters & a) // read motor currents
-{
- if (a.respond)
- a.com->printf ("rdi%.0f %.0f %.1f\r", MotorA.I.ave, MotorB.I.ave, Read_BatteryVolts ()); // Format good to be unpicked by cli in touch screen controller
-}
-
+/**
+* void rvi_cmd (struct parameters & a) // read last normalised motor voltage and current values sent to pwms
+*
+*/
void rvi_cmd (struct parameters & a) // read last normalised values sent to pwms
{
if (a.respond)
- a.com->printf ("rvi%.2f %.2f %.2f %.2f\r", MotorA.last_V, MotorA.last_I, MotorB.last_V, MotorB.last_I);
-}
-
-void fw_cmd (struct parameters & a) // Forward command
-{
- mode_set_both_motors (FORWARD, 0.0);
+ a.com->printf ("rvi%.2f %.2f %.2f %.2f\r%s", MotorA.last_V, MotorA.last_I, MotorB.last_V, MotorB.last_I, a.source == SOURCE_PC ? "\n" : "");
}
-void re_cmd (struct parameters & a) // Reverse command
+/**
+* void fw_cmd (struct parameters & a) // Forward command
+* Broadcast to all STM3_ESC boards, required to ACT but NOT respond
+*/
+void fw_cmd (struct parameters & a) // Forward command
{
- mode_set_both_motors (REVERSE, 0.0);
-}
-
-void rb_cmd (struct parameters & a) // Regen brake command
-{
- double b = a.dbl[0] / 100.0;
-// a.com->printf ("Applying brake %.3f\r\n", b);
- mode_set_both_motors (REGENBRAKE, b);
-// apply_brake (b);
+ mode_set_both_motors (MOTOR_FORWARD, 0.0);
+ if (a.source == SOURCE_PC)
+ pc.printf ("fw\r\n"); // Show response to action if command from pc terminal
}
-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 re_cmd (struct parameters & a) // Reverse command
+* Broadcast to all STM3_ESC boards, required to ACT but NOT respond
+*/
+void re_cmd (struct parameters & a) // Reverse command
{
- char t[36];
- for (int i = 0; i < 32; i++)
- t[i] = 0xff;
- for (int i = 0; i < 8191; i += 32) {
- a.com->printf (".");
- if (!wr_24LC64 (i, t, 32))
- a.com->printf ("eeprom write prob\r\n");
- }
-}
-/*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
- {'1', '9', '0', "Alternative ID ascii '1' to '9'"}, // defaults to '0' before eerom setup for first time
- {50, 250, 98, "Wheel diameter mm"}, // New 01/06/2018
- {10, 250, 27, "Motor pinion"}, // New 01/06/2018
- {50, 250, 85, "Wheel gear"}, // New 01/06/2018
-// last;
-} ;
-*/
-
-// New 22 June 2018
-// get bogie bytes - report back to touch controller
-void gbb_cmd (struct parameters & a) //
-{
- if (a.target_unit == BROADCAST || !a.resp_always) {
-// a.com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
- } else {
- pc.printf ("At gbb\r\n");
- char eeprom_contents[36]; // might need to be unsigned?
- memset (eeprom_contents, 0, 36);
- a.com->printf ("gbb");
- rd_24LC64 (0, eeprom_contents, 32);
- for (int i = 0; i < numof_eeprom_options; i++)
- a.com->printf (" %d", eeprom_contents[i]);
- a.com->putc ('\r');
- a.com->putc ('\n');
- }
+ mode_set_both_motors (MOTOR_REVERSE, 0.0);
+ if (a.source == SOURCE_PC)
+ pc.printf ("re\r\n");
}
-void mode_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents
+/**
+* void rb_cmd (struct parameters & a) // Regen brake command
+* Broadcast to all STM3_ESC boards, required to ACT but NOT respond
+*/
+void rb_cmd (struct parameters & a) // Regen brake command
{
- if (a.target_unit == BROADCAST || !a.resp_always) {
-// a.com->printf ("At mode_cmd, can not use BROADCAST with mode_cmd\r\n");
- } else {
- char t[36];
- a.com->printf ("At mode_cmd with node %d\r\n", a.target_unit);
- rd_24LC64 (0, t, 32);
- a.com->printf ("Numof params=%d\r\n", a.numof_dbls);
- for (int i = 0; i < numof_eeprom_options; i++)
- a.com->printf ("%2x\t%s\r\n", t[i], option_list[i].t);
- if (a.numof_dbls == 0) { // Read present contents, do not write
- a.com->printf ("That's it\r\n");
- } else { // Write new shit to eeprom
- a.com->printf ("\r\n");
- if (a.numof_dbls != numof_eeprom_options) {
- a.com->printf ("params required = %d, you offered %d\r\n", numof_eeprom_options, a.numof_dbls);
- } else { // Have been passed correct number of parameters
- int b;
- a.com->printf("Ready to write params to eeprom\r\n");
- for (int i = 0; i < numof_eeprom_options; 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
- a.com->printf("Warning - Parameter = %d, out of range, setting to default %d\r\n", b, option_list[i].def);
- b = option_list[i].def;
- }
- a.com->printf ("0x%2x\t%s\r\n", (t[i] = b), option_list[i].t);
- }
- wr_24LC64 (0, t, numof_eeprom_options);
- memcpy (mode_bytes,t,32);
- a.com->printf("Parameters set in eeprom\r\n");
- }
- }
- }
+ mode_set_both_motors (MOTOR_REGENBRAKE, a.dbl[0] / 100.0);
+ if (a.source == SOURCE_PC)
+ pc.printf ("rb %.2f\r\n", a.dbl[0] / 100.0);
}
-/*void coast_cmd (struct parameters & a) { // Coast
-}
+/**
+* void hb_cmd (struct parameters & a) // Hand brake command
+* Broadcast to all STM3_ESC boards, required to ACT but NOT respond
+*
+* NOTE Jan 2019 Hand brake not implemented
+*
*/
-void hb_cmd (struct parameters & a)
+void hb_cmd (struct parameters & a) // Hand brake command
{
if (a.respond) {
a.com->printf ("numof params = %d\r\n", a.numof_dbls);
a.com->printf ("Hand Brake : First %.3f, second %.3f\r\n", a.dbl[0], a.dbl[1]);
}
- mode_set_both_motors (HANDBRAKE, 0.0);
+ mode_set_both_motors (MOTOR_HANDBRAKE, 0.0);
}
-extern uint32_t last_temp_count;
+
+extern int numof_eeprom_options2 ;
+extern struct optpar const option_list2[] ;
+/**void mode_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents
+* mode_cmd called only from pc comms. No sense calling from Touch Screen Controller
+*
+* Called without parameters - Lists to pc terminal current settings
+*
+*/
+void mode19_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents
+{
+ char temps[36];
+ int i;
+ double topspeed; // New Jan 2019 - set max loco speed
+ pc.printf ("\r\nmode - Set system data in EEPROM - Jan 2019\r\nSyntax 'mode' with no parameters lists current state.\r\n");
+ if (a.numof_dbls) { // If more than 0 parameters supplied
+ for (i = 0; i < a.numof_dbls; i++)
+ temps[i] = (char)a.dbl[i]; // recast doubles to char
+ while (i < 33)
+ temps[i++] = 0;
+ switch ((int)a.dbl[0]) {
+ case 0: // MotorA_dir [0 or 1], MotorB_dir [0 or 1]
+ if (temps[1] == 0 || temps[1] == 1)
+ mode.wr(temps[1], MOTADIR);
+ if (temps[2] == 0 || temps[2] == 1)
+ mode.wr(temps[2], MOTBDIR);
+ break;
+ case 1: // MotorA_poles [4,6,8], MotorB_poles [4,6,8]
+ if (temps[1] == 4 || temps[1] == 6 || temps[1] == 8)
+ mode.wr(temps[1], MOTAPOLES);
+ if (temps[2] == 4 || temps[2] == 6 || temps[2] == 8)
+ mode.wr(temps[2], MOTBPOLES);
+ break;
+ case 2: // MotorA_ current sense resistors [1 to 4], MotorB_ current sense resistors [1 to 4]
+ if (temps[1] > 0 && temps[1] < 5)
+ mode.wr(temps[1], MOTADIR);
+ if (temps[2] > 0 && temps[2] < 5)
+ mode.wr(temps[2], MOTBDIR);
+ break;
+ case 3: // 2 Servo1 [0 or 1], Servo2 [0 or 1]
+ if (temps[1] == 0 || temps[1] == 1)
+ mode.wr(temps[1], SVO1);
+ if (temps[2] == 0 || temps[2] == 1)
+ mode.wr(temps[2], SVO2);
+ break;
+ case 4: // 3 RCIn1 [0 or 1], RCIn2 [0 or 1]
+ if (temps[1] == 0 || temps[1] == 1)
+ mode.wr(temps[1], RCIN1);
+ if (temps[2] == 0 || temps[2] == 1)
+ mode.wr(temps[2], RCIN2);
+ break;
+ case 5: // 4 Board ID '0' to '9'
+ if (temps[1] <= 9) // pointless to compare unsigned integer with zero
+ mode.wr('0' | temps[1], BOARD_ID);
+ break;
+ case 6: // TOP_SPEED
+ topspeed = a.dbl[1];
+ if (topspeed > 25.0) topspeed = 25.0;
+ if (topspeed < 1.0) topspeed = 1.0;
+ mode.wr((char)(topspeed * 10.0), TOP_SPEED);
+ break;
+ case 7: // 5 Wheel dia mm, Motor pinion teeth, Wheel gear teeth
+ mode.wr(temps[1], WHEELDIA);
+ mode.wr(temps[2], MOTPIN);
+ mode.wr(temps[3], WHEELGEAR);
+ break;
+ case 8: // {2, 5, 2, "Command source 2= COM2 (Touch Screen), 3= Pot, 4= RC Input1, 5= RC Input2"},
+ if (temps[1] > 1 && temps[1] < 6)
+ mode.wr(temps[1], COMM_SRC);
+ break;
+ case 83: // set to defaults
+ mode.set_defaults ();
+ break;
+ case 9: // 9 Save settings
+ mode.save ();
+ pc.printf ("Saving settings to EEPROM\r\n");
+ break;
+ default:
+ break;
+ } // endof switch
+ } // endof // If more than 0 parameters supplied
+ else {
+ pc.printf ("No Changes\r\n");
+ }
+ pc.printf ("mode 0\tMotorA_dir [0 or 1]=%d, MotorB_dir [0 or 1]=%d\r\n", mode.rd(MOTADIR), mode.rd(MOTBDIR));
+ pc.printf ("mode 1\tMotorA_poles [4,6,8]=%d, MotorB_poles [4,6,8]=%d\r\n", mode.rd(MOTAPOLES), mode.rd(MOTBPOLES));
+ pc.printf ("mode 2\tMotorA I 0R05 sense Rs [1to4]=%d, MotorB I 0R05 sense Rs [1to4]=%d\r\n", mode.rd(ISHUNTA), mode.rd(ISHUNTB));
+ pc.printf ("mode 3\tServo1 [0 or 1]=%d, Servo2 [0 or 1]=%d\r\n", mode.rd(SVO1), mode.rd(SVO2));
+ pc.printf ("mode 4\tRCIn1 [0 or 1]=%d, RCIn2 [0 or 1]=%d\r\n", mode.rd(RCIN1), mode.rd(RCIN2));
+ pc.printf ("mode 5\tBoard ID ['0' to '9']='%c'\r\n", mode.rd(BOARD_ID));
+ pc.printf ("mode 6\tTop Speed MPH [1.0 to 25.0]=%.1f\r\n", double(mode.rd(TOP_SPEED)) / 10.0);
+ pc.printf ("mode 7\tWheel dia mm=%d, Motor pinion teeth=%d, Wheel gear teeth=%d\r\n", mode.rd(WHEELDIA), mode.rd(MOTPIN), mode.rd(WHEELGEAR));
+ pc.printf ("mode 8\tCommand Src [%d] - 2=COM2 (Touch Screen), 3=Pot, 4=RC In1, 5=RC In2\r\n", mode.rd(COMM_SRC));
+ pc.printf ("mode 83\tSet to defaults\r\n");
+ pc.printf ("mode 9\tSave settings\r\r\n");
+
+}
+
+extern uint32_t last_temperature_count;
+/**
+* void temperature_cmd (struct parameters & a) {
+* Few boards have temperature sensor fitted. Non-preferred feature
+*/
void temperature_cmd (struct parameters & a) {
if (a.respond) {
- a.com->printf ("tem%c %d\r\n", mode_bytes[ID], (last_temp_count / 16) - 50);
+ a.com->printf ("tem%c %d\r\n", mode.rd(BOARD_ID), (last_temperature_count / 16) - 50);
}
}
-void bogie_constants_report_cmd (struct parameters & a) {
- if (a.respond) {
- a.com->printf ("bc%c %d %d %d\r\n", mode_bytes[ID], mode_bytes[WHEELDIA], mode_bytes[MOTPIN], mode_bytes[WHEELGEAR]);
- }
-}
-
+/**
+*void rpm_cmd (struct parameters & a) // to report e.g. RPM 1000 1000 ; speed for both motors
+*/
void rpm_cmd (struct parameters & a) // to report e.g. RPM 1000 1000 ; speed for both motors
{
if (a.respond)
- a.com->printf ("rpm%d %d\r", MotorA.RPM, MotorB.RPM);
+// a.com->printf ("rpm%c %d %d\r%s", mode.rd(BOARD_ID), MotorA.RPM, MotorB.RPM, a.source == SOURCE_PC ? "\n" : "");
+ a.com->printf ("rpm%c %.0f %.0f\r%s", mode.rd(BOARD_ID), MotorA.dRPM, MotorB.dRPM, a.source == SOURCE_PC ? "\n" : "");
}
-extern double rpm2mph ;
-void mph_cmd (struct parameters & a) // to report miles per hour
+/**
+*void mph_cmd (struct parameters & a) // to report miles per hour
+*/
+void mph_cmd (struct parameters & a) // to report miles per hour to 1 decimal place
{
if (a.respond)
- a.com->printf ("mph%c %.3f\r", mode_bytes[ID], (double)(MotorA.RPM + MotorB.RPM) * rpm2mph / 2.0);
+// a.com->printf ("mph%c %.1f\r%s", mode.rd(BOARD_ID), (double)(MotorA.RPM + MotorB.RPM) * rpm2mph / 2.0, a.source == SOURCE_PC ? "\n" : "");
+ a.com->printf ("mph%c %.1f\r%s", mode.rd(BOARD_ID), (double)(MotorA.dMPH + MotorB.dMPH) / 2.0, a.source == SOURCE_PC ? "\n" : "");
+}
+
+/**
+*void sysV_report (struct parameters & a) // to report system voltage
+* Reports system link voltage to one decimal place
+*/
+void sysV_report (struct parameters & a) //
+{
+ if (a.respond)
+ a.com->printf ("?v%c %.1f\r%s", mode.rd(BOARD_ID), Read_BatteryVolts(), a.source == SOURCE_PC ? "\n" : "");
}
-void menucmd (struct parameters & a);
+/**
+*void sysI_report (struct parameters & a) // to report motor currents
+* Reports doubles for each motor current amps to 2 decimal places
+*/
+void sysI_report (struct parameters & a) //
+{
+ if (a.respond) // Calibration, refinement of 6000.0 (not miles out) first guess possible.
+// a.com->printf ("?i%c %.2f %.2f\r%s", mode_bytes[BOARD_ID], (double)MotorA.I.ave / 6000.0, (double)MotorB.I.ave / 6000.0, a.source == SOURCE_PC ? "\n" : "");
+// a.com->printf ("?i%c %.2f %.2f\r%s", mode.rd(BOARD_ID), (double)MotorA.I.ave / 6000.0, MotorA.I.dblave2 / 6000.0, a.source == SOURCE_PC ? "\n" : "");
+ a.com->printf ("?i%c %.2f %.2f\r%s", mode.rd(BOARD_ID), MotorA.Idbl, MotorB.Idbl, a.source == SOURCE_PC ? "\n" : "");
+}
+
+/**void vi_cmd (struct parameters & a)
+*
+* For setting motor voltage and current limits from pc terminal for test
+*/
void vi_cmd (struct parameters & a)
{
-// 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);
+ pc.printf ("setVI from %s\r\n", a.source == SOURCE_PC ? "PC" : "Touch Screen");
}
+/**
+*void v_cmd (struct parameters & a)
+* Set motor voltage limit from either source without checking for addressed board
+*/
void v_cmd (struct parameters & a)
{
-// if (a.respond)
-// com->printf ("In setV, setting V to %.2f\r\n", a.dbl[0]);
- setV (a.dbl[0] / 100.0);
+ MotorA.set_V_limit (a.dbl[0] / 100.0);
+ MotorB.set_V_limit (a.dbl[0] / 100.0);
}
+/**
+*void i_cmd (struct parameters & a)
+* Set motor current limit from either source without checking for addressed board
+*/
void i_cmd (struct parameters & a)
{
-// if (a.respond)
-// a.com->printf ("In setI, setting I to %.2f\r\n", a.dbl[0]);
- setI (a.dbl[0] / 100.0);
+ MotorA.set_I_limit (a.dbl[0] / 100.0);
+ MotorB.set_I_limit (a.dbl[0] / 100.0);
}
-void kd_cmd (struct parameters & a) // kick the watchdog
+/**
+*void kd_cmd (struct parameters & a) // kick and enable the watch dog
+*
+*/
+void kd_cmd (struct parameters & a) // kick the watchdog. Reached from TS or pc.
{
- WatchDog = WATCHDOG_RELOAD + (I_Am() & 0x0f);
-// a.com->printf ("Poked %d up Dog\r\n", WatchDog);
+ WatchDog = WATCHDOG_RELOAD + (mode.rd(BOARD_ID) & 0x0f); // Reload watchdog timeout. Counted down @ 8Hz
+ WatchDogEnable = true; // Receipt of this command sufficient to enable watchdog
}
-void who_cmd (struct parameters & a)
+/**
+*void who_cmd (struct parameters & a) // Reachable always from pc. Only addressed board responds to TS
+*
+* When using STM3_ESC boards together with 'Brute Touch Screen Controller', controller needs to identify number and identity
+* of all connected STM3_ESC boards. To do this the Touch Screen issues '0who', '1who' ... '9who' allowing time between each
+* for an identified STM3_ESC to respond with 'who7' (if it was number 7) without causing contention of paralleled serial opto isolated bus
+*/
+void who_cmd (struct parameters & a) // Reachable always from pc. Only addressed board responds to TS
{
- int i = I_Am ();
- if (I_Am() == a.target_unit)
- a.com->printf ("who%c\r\n", a.target_unit);
+ if (a.source == SOURCE_PC || mode.rd(BOARD_ID) == a.target_unit)
+ a.com->printf ("who%c\r%s", mode.rd(BOARD_ID), a.source == SOURCE_PC ? "\n" : "");
}
-extern void rcin_report () ;
+/**
+*void rcin_pccmd (struct parameters & a)
+*
+* For test, reports to pc terminal info about radio control input channels
+*/
void rcin_pccmd (struct parameters & a)
{
rcin_report ();
}
-struct kb_command {
- const char * cmd_word; // points to text e.g. "menu"
- const char * explan;
- void (*f)(struct parameters &); // points to function
-} ;
+void scmd (struct parameters & a) // filter coefficient fiddler
+{
+ switch ((int)a.dbl[0]) {
+ case 1:
+ MotorA.s[1] = MotorB.s[1] = a.dbl[1];
+ break;
+ case 2:
+ MotorA.s[2] = MotorB.s[2] = a.dbl[1];
+ break;
+ case 3:
+ MotorA.s[3] = MotorB.s[3] = a.dbl[1];
+ break;
+ case 4:
+ MotorA.s[4] = MotorB.s[4] = a.dbl[1];
+ break;
+ case 5:
+ MotorA.set_speed (a.dbl[1]);
+ MotorB.set_speed (a.dbl[1]);
+ break;
+ default:
+ pc.printf ("Wrong use of scmd %f\r\n", a.dbl[0]);
+ }
+ pc.printf ("Filter Coeffs 1 to 4\r\n");
+ pc.printf ("1 %.3f\tPscale 0.01-0.5\r\n", MotorA.s[1]);
+ pc.printf ("2 %.3f\tP_gain 1.0-1000.0\r\n", MotorA.s[2]);
+ pc.printf ("3 %.3f\tDscale 0.01-0.5\r\n", MotorA.s[3]);
+ pc.printf ("4 %.3f\tD_gain 1.0-1000.0\r\n", MotorA.s[4]);
+ pc.printf ("5 Set target_speed\r\n");
+}
+
+ struct kb_command { // Commands tabulated as list of these structures as seen below
+ const char * cmd_word; // points to text e.g. "menu"
+ const char * explan; // very brief explanation or clue as to purpose of function
+ void (*f)(struct parameters &); // points to function
+} ; // Positioned in code here as knowledge needed by following menucmd
+
+/**
+* void menucmd (struct parameters & a)
+*
+* List available terminal commands to pc terminal. No sense in touch screen using this
+*/
+void menucmd (struct parameters & a)
+{
+ if (a.respond) {
+ a.com->printf("\r\n\nDual BLDC ESC type STM3 2018\r\nAt menucmd function - listing commands, source %s:-\r\n", a.source == SOURCE_PC ? "PC" : "TS");
+ for(int i = 0; i < a.numof_menu_items; i++)
+ a.com->printf("[%s]\t\t%s\r\n", a.command_list[i].cmd_word, a.command_list[i].explan);
+ a.com->printf("End of List of Commands\r\n");
+ }
+}
+
+/********************** END OF COMMAND LINE INTERPRETER COMMANDS *************************************/
/**
-struct kb_command const loco_command_list[] = {
-List of commands accepted from external controller through opto isolated com port 9600, 8,n,1
+* struct kb_command const loco_command_list[] = {
+* List of commands accepted from external controller through opto isolated com port 19200, 8,n,1
*/
-struct kb_command const loco_command_list[] = {
- {"ls", "Lists available commands", menucmd},
- {"?", "Lists available commands, same as ls", menucmd},
+struct kb_command const loco_command_list[] = { // For comms between STM3_ESC and 'Brute Touch Screen Controller'
+ // ***** Broadcast commands for all STM3_ESC boards to execute. Boards NOT to send serial response *****
{"fw", "forward", fw_cmd},
{"re", "reverse", re_cmd},
{"rb", "regen brake 0 to 99 %", rb_cmd},
@@ -302,23 +449,19 @@
{"v", "set motors V percent RANGE 0 to 100", v_cmd},
{"i", "set motors I percent RANGE 0 to 100", i_cmd},
{"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
+ {"kd", "kick the dog, reloads WatchDog", kd_cmd},
+ // ***** Endof Broadcast commands for all STM3_ESC boards to execute. Boards NOT to send serial response *****
+
+ // ***** Following are rx'd requests for serial response from addressed STM3_ESC only
+ {"?v", "Report system bus voltage", sysV_report},
+ {"?i", "Report motor both currents", sysI_report},
{"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd},
- {"mode", "read or set params in eeprom", mode_cmd},
- {"erase", "set eeprom contents to all 0xff", erase_cmd},
{"tem", "report temperature", temperature_cmd},
- {"kd", "kick the dog, reloads WatchDog", kd_cmd},
- {"wden", "enable watchdog if modes allow", wden_lococmd},
- {"wddi", "disable watchdog always", wddi_lococmd},
- {"rpm", "read motor pair speeds", rpm_cmd},
{"mph", "read loco speed miles per hour", mph_cmd},
- {"rvi", "read most recent values sent to pwms", rvi_cmd},
- {"rdi", "read motor currents and power voltage", rdi_cmd},
- {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd}, // OBSOLETE, replaced by 'gbb'
- {"gbb", "get bogie bytes from eeprom and report", gbb_cmd},
- {"nu", "do nothing", null_cmd},
-};
-
-//const int numof_loco_menu_items = sizeof(loco_command_list) / sizeof(kb_command);
+// {"rvi", "read most recent values sent to pwms", rvi_cmd},
+// {"rdi", "read motor currents and power voltage", rdi_cmd},
+ // ***** Endof
+} ;
/**
@@ -328,7 +471,10 @@
struct kb_command const pc_command_list[] = {
{"ls", "Lists available commands", menucmd},
{"?", "Lists available commands, same as ls", menucmd},
+#ifdef USING_DC_MOTORS
{"mtypes", "report types of motors found", mt_cmd},
+#endif
+ {"s","1-4, filter param", scmd},
{"pot", "read drivers control pot", pot_cmd},
{"fw", "forward", fw_cmd},
{"re", "reverse", re_cmd},
@@ -337,195 +483,120 @@
{"v", "set motors V percent RANGE 0 to 100", v_cmd},
{"i", "set motors I percent RANGE 0 to 100", i_cmd},
{"vi", "set motors V and I percent RANGE 0 to 100", vi_cmd},
+ {"?v", "Report system bus voltage", sysV_report},
+ {"?i", "Report motor both currents", sysI_report},
{"who", "search for connected units, e.g. 3who returs 'who3' if found", who_cmd},
- {"mode", "read or set params in eeprom", mode_cmd},
- {"erase", "set eeprom contents to all 0xff", erase_cmd},
- {"tem", "report temperature", temperature_cmd},
+ {"mode", "read or set params in eeprom", mode19_cmd}, // Big change Jan 2019
+// {"erase", "set eeprom contents to all 0xff", erase_cmd},
+ {"tem", "report temperature", temperature_cmd}, // Reports -50 when sensor not fitted
{"kd", "kick the dog, reloads WatchDog", kd_cmd},
- {"wden", "enable watchdog if modes allow", wden_pccmd},
- {"wddi", "disable watchdog always", wddi_pccmd},
+ {"ver", "Version", ver_cmd},
{"rcin", "Report Radio Control Input stuff", rcin_pccmd},
{"rpm", "read motor pair speeds", rpm_cmd},
{"mph", "read loco speed miles per hour", mph_cmd},
{"rvi", "read most recent values sent to pwms", rvi_cmd},
{"rdi", "read motor currents and power voltage", rdi_cmd},
- {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
- {"gbb", "get bogie bytes from eeprom and report", gbb_cmd}, // OBSOLETE, replaced by 'gbb'
+// {"bc", "bogie constants - wheel dia, motor pinion, wheel gear", bogie_constants_report_cmd},
+// {"gbb", "get bogie bytes from eeprom and report", gbb_cmd}, // OBSOLETE, replaced by 'gbb'
{"nu", "do nothing", null_cmd},
-};
+} ;
-void setup_comms () {
- pccom.com = & pc;
- pccom.command_list = pc_command_list;
- pccom.numof_menu_items = sizeof(pc_command_list) / sizeof(kb_command);
- pccom.cl_index = 0;
- pccom.gp_i = 0; // general puropse integer, not used to 30/4/2018
- pccom.resp_always = true;
- lococom.com = & com2;
- lococom.command_list = loco_command_list;
- lococom.numof_menu_items = sizeof(loco_command_list) / sizeof(kb_command);
- lococom.cl_index = 0;
- lococom.gp_i = 0; // general puropse integer, toggles 0 / 1 to best guess source of rpm
- lococom.resp_always = false;
-}
-
-
-void menucmd (struct parameters & a)
-{
- if (a.respond) {
- a.com->printf("\r\n\nDual BLDC ESC type STM3 2018\r\nAt menucmd function - listing commands:-\r\n");
- for(int i = 0; i < a.numof_menu_items; i++)
- a.com->printf("[%s]\t\t%s\r\n", a.command_list[i].cmd_word, a.command_list[i].explan);
- a.com->printf("End of List of Commands\r\n");
- }
-}
+// cli_2019 (BufferedSerial * comport, kb_command const * list, int list_len, int source) {
+/**
+* cli_2019 pcc (&pc, pc_command_list, sizeof(pc_command_list) / sizeof(kb_command), SOURCE_PC) ;
+* cli_2019 tsc (&com2, loco_command_list, sizeof(loco_command_list) / sizeof(kb_command), SOURCE_TS) ;
+*
+* Instantiate two Command Line Interpreters, one for pc terminal and one for touch screen controller
+*/
+cli_2019 pcc (&pc, pc_command_list, sizeof(pc_command_list) / sizeof(kb_command), SOURCE_PC) ;
+cli_2019 tsc (&com2, loco_command_list, sizeof(loco_command_list) / sizeof(kb_command), SOURCE_TS) ;
/*
New - March 2018
-Using opto isolated serial port, paralleled up using same pair to multiple boards running this code.
+Using opto isolated serial port, paralleled up using same pair to multiple STM3_ESC 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, 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 cli_core (struct parameters & a) {
- const int MAX_CMD_LEN = 180;
- int ch, IAm = I_Am();
+
+/**
+* void cli_2019::test () {
+*
+* Daft check that class instantiation worked
+*/
+void cli_2019::test () {
+ pc.printf ("At cli2019::test, source %d len %d,\r\n", a.source, a.numof_menu_items);
+}
+
+/**
+* void cli_2019::core () {
+*
+* Command Line Interpreter.
+* This to be called every few milli secs from main programme loop.
+* Reads any rx'd chars into command line buffer, returns when serial buffer empty.
+* If last char rx'd war '\r' end of text delimiter, apt command_list is searched for a matched command in command line
+* If matched command found, apt function is executed.
+* Parameters available to functions from 'parameters' struct.
+*/
+void cli_2019::core () {
+ int ch, IAm = mode.rd(BOARD_ID);
char * pEnd;//, * cmd_line_ptr;
while (a.com->readable()) {
ch = a.com->getc();
- if (a.cl_index > MAX_CMD_LEN) { // trap out stupidly long command lines
+ if (clindex > MAX_CMD_LEN) { // trap out stupidly long command lines
+ ESC_Error.set (FAULT_COM_LINE_LEN, 1); // Set FAULT_EEPROM bit 0 if 24LC64 problem
a.com->printf ("Error!! Stupidly long cmd line\r\n");
- a.cl_index = 0;
+ clindex = 0;
}
if(ch != '\r') { // was this the 'Enter' key?
if (ch != '\n') // Ignore line feeds
- a.cmd_line[a.cl_index++] = ch; // added char to command being assembled
+ cmdline[clindex++] = ch; // added char to command being assembled
}
else { // key was CR, may or may not be command to lookup
a.target_unit = BROADCAST; // Set to BROADCAST default once found command line '\r'
- a.cmd_line_ptr = a.cmd_line;
- a.cmd_line[a.cl_index] = 0; // null terminate command string
- if(a.cl_index) { // If have got some chars to lookup
+ cmdline_ptr = cmdline;
+ cmdline[clindex] = 0; // null terminate command string
+ if(clindex) { // If have got some chars to lookup
int i, wrdlen;
- if (isdigit(a.cmd_line[0])) { // Look for command with prefix digit
- a.cmd_line_ptr++; // point past identified digit prefix
- a.target_unit = a.cmd_line[0]; // '0' to '9'
+ if (isdigit(cmdline[0])) { // Look for command with prefix digit
+ cmdline_ptr++; // point past identified digit prefix
+ a.target_unit = cmdline[0]; // '0' to '9'
//com->printf ("Got prefix %c\r\n", cmd_line[0]);
}
for (i = 0; i < a.numof_menu_items; i++) { // Look for input match in command list
- wrdlen = strlen(a.command_list[i].cmd_word);
- if(strncmp(a.command_list[i].cmd_word, a.cmd_line_ptr, wrdlen) == 0 && !isalpha(a.cmd_line_ptr[wrdlen])) { // If match found
+ wrdlen = strlen(commandlist[i].cmd_word);
+ if(strncmp(commandlist[i].cmd_word, cmdline_ptr, wrdlen) == 0 && !isalpha(cmdline_ptr[wrdlen])) { // If match found
for (int k = 0; k < MAX_PARAMS; k++) {
a.dbl[k] = 0.0;
}
a.position_in_list = i;
a.numof_dbls = 0;
- pEnd = a.cmd_line_ptr + wrdlen;
+ pEnd = cmdline_ptr + wrdlen;
while (*pEnd) { // Assemble all numerics as doubles
a.dbl[a.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]);
-// param_block.times[i] = clock();
-// a.respond = false;
a.respond = a.resp_always;
if (((a.target_unit == BROADCAST) && (IAm == '0')) || (IAm == a.target_unit))
a.respond = true; // sorted 26/4/18
// All boards to obey BROADCAST command, only specific board to obey number prefixed command
if ((a.target_unit == BROADCAST) || (IAm == a.target_unit))
- a.command_list[i].f(a); // execute command if addressed to this unit
+ commandlist[i].f(a); // execute command if addressed to this unit
i = a.numof_menu_items + 1; // to exit for loop
} // end of match found
} // End of for numof_menu_items
- if(i == a.numof_menu_items)
- a.com->printf("No Match Found for CMD [%s]\r\n", a.cmd_line);
+ if(i == a.numof_menu_items) {
+// a.com->printf("No Match Found for CMD [%s]\r\n", cmdline);
+ pc.printf("No Match Found for CMD [%s]\r\n", cmdline);
+ ESC_Error.set (FAULT_COM_LINE_NOMATCH, 1); // Set FAULT_EEPROM bit 0 if 24LC64 problem
+ }
} // End of If have got some chars to lookup
- //com->printf("\r\n>");
- a.cl_index = 0;
- } // End of else key was CR, may or may not be command to lookup
- } // End of while (com->readable())
-}
-
-void command_line_interpreter_pc () {
- cli_core (pccom);
-}
-void command_line_interpreter_loco () {
- cli_core (lococom);
-}
-
-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, 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 < a.numof_menu_items; i++) { // Look for input match in command list
- wrdlen = strlen(a.command_list[i].cmd_word);
- if(strncmp(a.command_list[i].cmd_word, a.cmd_line_ptr, wrdlen) == 0 && !isalpha(a.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++;
- }
- }
- //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();
- 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
-// }*/
-}
+ clindex = 0;
+ } // End of else key was CR, may or may not be command to lookup
+ } // End of while (com->readable())
+} // end of command line interpreter core function
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/error_handler.cpp Mon Mar 04 17:51:08 2019 +0000
@@ -0,0 +1,87 @@
+#include "mbed.h"
+#include "DualBLS.h"
+#include "BufferedSerial.h"
+extern BufferedSerial pc;
+
+/*class error_handling_Jan_2019
+{
+ int32_t TS_fault[NUMOF_REPORTABLE_TS_ERRORS] ; // Some number of reportable error codes, accessible through set and read members
+ public:
+ error_handling_Jan_2019 () { // default constructor
+ for (int i = 0; i < (sizeof(TS_fault) / sizeof(int32_t)); i++)
+ TS_fault[i] = 0;
+ }
+ void set (uint32_t, int32_t) ;
+ uint32_t read (uint32_t) ;
+ bool all_good () ;
+ void report_any () ;
+} ;
+*/
+
+const char * FaultList[] = {
+/*
+ FAULT_0,
+ FAULT_EEPROM,
+ FAULT_BOARD_ID,
+ FAULT_COM_LINE_LEN,
+ FAULT_COM_LINE_NOMATCH,
+ FAULT_COM_LINE_LEN_PC,
+ FAULT_COM_LINE_LEN_TS,
+ FAULT_COM_LINE_NOMATCH_PC,
+ FAULT_COM_LINE_NOMATCH_TS,
+ FAULT_MAX,
+ NUMOF_REPORTABLE_TS_ERRORS
+*/
+ "Zero",
+ "EEPROM",
+ "board ID",
+ "com line len",
+ "com line nomatch",
+ "com line len",
+ "com line len",
+ "com no match",
+ "com no match",
+ "max",
+ "endoflist",
+ " ",
+ } ;
+
+bool error_handling_Jan_2019::all_good () {
+ for (int i = 0; i < NUMOF_REPORTABLE_TS_ERRORS; i++)
+ if (ESC_fault[i])
+ return false;
+ return true;
+}
+
+/**void error_handling_Jan_2019::set (uint32_t err_no, int32_t bits_to_set) {
+ Used to set bits in error int
+ Uses OR to set new bits without clearing other bits set previously
+*/
+void error_handling_Jan_2019::set (uint32_t err_no, int32_t bits_to_set) {
+ if (bits_to_set) {
+ pc.printf ("At Error.set, err_no %d, bits %lx\r\n", err_no, bits_to_set);
+ ESC_fault[err_no] |= bits_to_set; // Uses OR to set new bits without clearing other bits set previously
+ }
+}
+
+/**void error_handling_Jan_2019::clr (uint32_t err_no) {
+ Used to clear all bits in error int
+*/
+void error_handling_Jan_2019::clr (uint32_t err_no) {
+ ESC_fault[err_no] = 0;
+}
+
+uint32_t error_handling_Jan_2019::read (uint32_t err_no) {
+ return ESC_fault[err_no];
+}
+
+void error_handling_Jan_2019::report_any (bool retain) {
+ for (int i = 0; i < NUMOF_REPORTABLE_TS_ERRORS; i++) {
+ if (ESC_fault[i]) {
+ pc.printf ("Error report, number %d, value %d, %s\r\n", i, ESC_fault[i], FaultList[i]);
+ if (!retain)
+ ESC_fault[i] = 0;
+ }
+ }
+}
+
--- a/main.cpp Sat Jan 19 11:45:01 2019 +0000
+++ b/main.cpp Mon Mar 04 17:51:08 2019 +0000
@@ -12,14 +12,23 @@
/*
Brushless_STM3 board
-
+Jan 2019 * WatchDog implemented. Default is disabled, 'kd' command from TS controller enables and reloads
+ * Tidied brushless_motor class, parameter passing now done properly
+ * class RControl_In created. Inputs now routed to new pins, can now use two chans both class RControl_In and Servo out
+ (buggery board required for new inputs)
+ * Added version string
+ * Error handler written and included
+ * Realised Nanotec motors are 6 pole, all others are 8 pole. Modified 'mode' to include 'motor poles' in EEPROM data, now speed reading correct for all
+ * Reorganised EEPROM data - mode setting now much easier, less error prone
+ * Maximum speed now one EEPROM option, range 1.0 to 25.0 MPH
+
New 29th May 2018 **
LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable
*/
/* STM32F401RE - compile using NUCLEO-F401RE
-// PROJECT - Dual Brushless Motor Controller - Jon Freeman June 2018.
+// PROJECT - STM3_ESC Dual Brushless Motor Controller - Jon Freeman June 2018.
AnalogIn to read each motor current
@@ -44,6 +53,7 @@
#include "F446ZE.h" // A thought for future version
#endif
/* Global variable declarations */
+char const const_version_string[] = {"1.0.0\0"}; // Version string, readable from serial ports
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
bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable
@@ -51,80 +61,31 @@
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;
-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
bool temp_sensor_exists = false;
-bool eeprom_flag; // gets set according to 24LC674 being found or not
-bool mode_good_flag = false;
-char mode_bytes[36];
+double rpm2mph;
uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01
- last_temp_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
-// struct single_bogie_options bogie;
-double rpm2mph = 0.0; // gets calculated from eeprom mode entries if present
+ last_temperature_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
/* End of Global variable declarations */
Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc
Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
Ticker temperature_find_ticker;
Timer temperature_timer;
+
+#ifdef USING_DC_MOTORS
Timer dc_motor_kicker_timer;
Timeout motors_restore;
+#endif
+
RControl_In RC_chan_1 (PC_14);
RControl_In RC_chan_2 (PC_15); // Instantiate two radio control input channels and specify which InterruptIn pin
-
-
-// Interrupt Service Routines
-void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec
-{
- static bool readflag = false;
- int t = temperature_timer.read_ms ();
- if ((t == 5) && (!readflag)) {
- last_temp_count = temp_sensor_count;
- temp_sensor_count = 0;
- readflag = true;
- }
- if (t == 6)
- readflag = false;
-}
+error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes.
-/** void ISR_loop_timer ()
-* This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
-* This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
-* Increments global 'sys_timer', usable anywhere as general measure of elapsed time
-*/
-void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
-{
- loop_flag = true; // set flag to allow main programme loop to proceed
- sys_timer++; // Just a handy measure of elapsed time for anything to use
- if ((sys_timer & 0x03) == 0)
- flag_8Hz = true;
-}
-
-/** void ISR_voltage_reader ()
-* This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
-*
-* AtoD_reader() called from convenient point in code to take readings outside of ISRs
-*/
-
-void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50
-{
- AtoD_Semaphore++;
- fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
-}
-
-void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some
-{
- int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ?
- temperature_timer.reset ();
- temp_sensor_count++;
- if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
- temp_sensor_count++;
-// T2 = !T2; // scope hanger
-}
-
-// End of Interrupt Service Routines
+eeprom_settings mode (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup
//uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
/*
@@ -159,15 +120,63 @@
KEEP_L_MASK_B, KEEP_H_MASK_B
} ;
-brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK);
-brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK);
+brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK, ISHUNTA);
+brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB);
-void report_motor_types () // not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise
+extern cli_2019 pcc, tsc; // command line interpreters from pc and touch screen
+
+// Interrupt Service Routines
+void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec
+{
+ static bool readflag = false;
+ int t = temperature_timer.read_ms ();
+ if ((t == 5) && (!readflag)) {
+ last_temperature_count = temp_sensor_count;
+ temp_sensor_count = 0;
+ readflag = true;
+ }
+ if (t == 6)
+ readflag = false;
+}
+
+/** void ISR_loop_timer ()
+* This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
+* This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
+* Increments global 'sys_timer', usable anywhere as general measure of elapsed time
+*/
+void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US
{
- pc.printf ("Mot A is %s, Mot B is %s\r\n", MotorA.motor_is_brushless() ? "Brushless":"DC", MotorB.motor_is_brushless() ? "Brushless":"DC");
+ loop_flag = true; // set flag to allow main programme loop to proceed
+ sys_timer++; // Just a handy measure of elapsed time for anything to use
+ if ((sys_timer & 0x03) == 0)
+ flag_8Hz = true;
}
+/** void ISR_voltage_reader ()
+* This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
+*
+* AtoD_reader() called from convenient point in code to take readings outside of ISRs
+*/
+void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50
+{
+ AtoD_Semaphore++;
+ fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
+}
+
+void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some
+{
+ int t = temperature_timer.read_us (); // Must be being overrun by something, most likely culprit A-D reading ?
+ temperature_timer.reset ();
+ temp_sensor_count++;
+ if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
+ temp_sensor_count++;
+// T2 = !T2; // scope hanger
+}
+
+// End of Interrupt Service Routines
+
+
void setVI (double v, double i)
{
MotorA.set_V_limit (v); // Sets max motor voltage
@@ -176,40 +185,18 @@
MotorB.set_I_limit (i);
}
-void setV (double v)
-{
- MotorA.set_V_limit (v);
- MotorB.set_V_limit (v);
-}
-
-void setI (double i)
-{
- MotorA.set_I_limit (i);
- MotorB.set_I_limit (i);
-}
-
/**
-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
+* 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;
-// if (MotorA.dc_motor) {
-// MotorA.low_side_on ();
-// }
-// else {
if (MotorA.tickleon)
MotorA.high_side_off ();
-// }
-// if (MotorB.dc_motor) {
-// MotorB.low_side_on ();
-// }
-// else {
if (MotorB.tickleon)
MotorB.high_side_off ();
-// }
if (AtoD_Semaphore > 20) {
pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
AtoD_Semaphore = 20;
@@ -227,7 +214,7 @@
driverpot_reading >>= 1;
break;
case 2:
- MotorA.sniff_current (); // Initiate ADC reading into averaging table
+ MotorA.sniff_current (); // Initiate ADC current reading
break;
case 3:
MotorB.sniff_current ();
@@ -238,12 +225,10 @@
i = 0;
} // end of while (AtoD_Semaphore > 0) {
if (MotorA.tickleon) {
-// if (MotorA.dc_motor || MotorA.tickleon) {
MotorA.tickleon--;
MotorA.motor_set (); // Reactivate any high side switches turned off above
}
if (MotorB.tickleon) {
-// if (MotorB.dc_motor || MotorB.tickleon) {
MotorB.tickleon--;
MotorB.motor_set ();
}
@@ -295,11 +280,12 @@
return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
}
+
void mode_set_both_motors (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 (mode == MOTOR_REGENBRAKE) {
if (val > 1.0)
val = 1.0;
if (val < 0.0)
@@ -310,36 +296,9 @@
}
}
-extern void setup_comms () ;
-extern void command_line_interpreter_pc () ;
-extern void command_line_interpreter_loco () ;
-extern int check_24LC64 () ; // Call from near top of main() to init i2c bus
-extern bool wr_24LC64 (int mem_start_addr, char * source, int length) ;
-extern bool rd_24LC64 (int mem_start_addr, char * dest, int length) ;
+
-/*struct motorpairoptions { // This to be user settable in eeprom, 32 bytes
- uint8_t MotA_dir, // 0 or 1
- MotB_dir, // 0 or 1
- gang, // 0 for separate control (robot mode), 1 for ganged loco bogie mode
- serv1, // 0, 1, 2 = Not used, Input, Output
- serv2, // 0, 1, 2 = Not used, Input, Output
- cmd_source, // 0 Invalid, 1 COM1, 2 COM2, 3 Pot, 4 Servo1, 5 Servo2
- last;
-} ;
-*/
-int I_Am () // Returns boards id number as ASCII char
-{
- return IAm;
-}
-
-double mph (int rpm)
-{
- if (mode_good_flag) {
- return rpm2mph * (double) rpm;
- }
- return -1.0;
-}
-
+#ifdef USING_DC_MOTORS
void restorer ()
{
motors_restore.detach ();
@@ -354,6 +313,7 @@
MotorB.motor_set ();
}
}
+#endif
void rcin_report () {
double c1 = RC_chan_1.normalised();
@@ -429,14 +389,14 @@
case HAND_CONT_BRAKE_WAIT: // Only get here after direction input changed or newly validated at power on
pc.printf ("At HAND_CONT_BRAKE_WAIT\r\n");
brake_effort = 0.05; // Apply braking not too fiercely
- mode_set_both_motors (REGENBRAKE, brake_effort); // Direction change
+ mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change
hand_controller_state = HAND_CONT_BRAKE_POT;
break;
case HAND_CONT_BRAKE_POT: // Only get here after one pass through HAND_CONT_BRAKE_WAIT but
if (brake_effort < 0.9) { // remain in this state until driver has turned pott fully anticlockwise
brake_effort += 0.05; // ramp braking up to max over about one sec after direction change or validation
- mode_set_both_motors (REGENBRAKE, brake_effort); // Direction change or testing at power on
+ mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // Direction change or testing at power on
pc.printf ("Brake effort %.2f\r\n", brake_effort);
}
else { // once braking up to quite hard
@@ -450,7 +410,7 @@
brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
if (brake_effort < 0.0)
brake_effort = 0.5;
- mode_set_both_motors (REGENBRAKE, brake_effort);
+ mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort);
old_pot_position = pot_position;
hand_controller_state = HAND_CONT_STATE_BRAKING;
pc.printf ("Brake\r\n");
@@ -463,7 +423,7 @@
if (worth_the_bother(pot_position, old_pot_position)) {
old_pot_position = pot_position;
brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
- mode_set_both_motors (REGENBRAKE, brake_effort);
+ mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort);
// pc.printf ("Brake %.2f\r\n", brake_effort);
}
}
@@ -472,9 +432,9 @@
case HAND_CONT_STATE_INTO_DRIVING: // Only get here after HAND_CONT_STATE_BRAKING
pc.printf ("Drive\r\n");
if (direction_new == 1)
- mode_set_both_motors (FORWARD, 0.0); // sets both motors
+ mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors
else
- mode_set_both_motors (REVERSE, 0.0);
+ mode_set_both_motors (MOTOR_REVERSE, 0.0);
hand_controller_state = HAND_CONT_STATE_DRIVING;
break;
@@ -502,7 +462,6 @@
int eighth_sec_count = 0;
double servo_angle = 0.0; // For testing servo outs
-
Temperature_pin.fall (&temp_sensor_isr);
Temperature_pin.mode (PullUp);
// Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
@@ -512,83 +471,53 @@
// Done setting up system interrupt timers
temperature_timer.start ();
- pc.baud (9600);
- com3.baud (1200);
- com2.baud (19200);
- setup_comms ();
+ pc.baud (9600); // COM port to pc
+ com3.baud (1200); // Once had an idea to use this for IR comms, never tried
+ com2.baud (19200); // Opto isolated serial bus connecting 'n' STM3_ESC boards to 1 Brute Touch Screen controller
- IAm = '0';
- if (check_24LC64() != 0xa0) { // searches for i2c devices, returns address of highest found
- pc.printf ("Check for 24LC64 eeprom FAILED\r\n");
- com2.printf ("Check for 24LC64 eeprom FAILED\r\n");
- eeprom_flag = false;
- } else { // Found 24LC64 memory on I2C
- eeprom_flag = true;
- bool k;
- k = rd_24LC64 (0, mode_bytes, 32);
- if (!k)
- com2.printf ("Error reading from eeprom\r\n");
+ rpm2mph = 60.0 // to Motor Revs per hour;
+ * ((double)mode.rd(MOTPIN) / (double)mode.rd(WHEELGEAR)) // Wheel revs per hour
+ * PI * ((double)mode.rd(WHEELDIA) / 1000.0) // metres per hour
+ * 39.37 / (1760.0 * 36.0); // miles per hour
-// int err = 0;
- for (int i = 0; i < numof_eeprom_options; i++) {
- if ((mode_bytes[i] < option_list[i].min) || (mode_bytes[i] > option_list[i].max)) {
- com2.printf ("EEROM error with %s\r\n", option_list[i].t);
- pc.printf ("EEROM error with %s\r\n", option_list[i].t);
- if (i == ID) { // need to force id to '0'
- pc.printf ("Bad board ID value %d, forcing to \'0\'\r\n");
- mode_bytes[ID] = '0';
- }
-// err++;
- }
-// else
-// com2.printf ("%2x Good %s\r\n", buff[i], option_list[i].t);
- }
- rpm2mph = 0.0;
- if (mode_bytes[WHEELGEAR] == 0) // avoid making div0 error
- mode_bytes[WHEELGEAR]++;
-// if (err == 0) {
- mode_good_flag = true;
- MotorA.direction_set (mode_bytes[MOTADIR]);
- MotorB.direction_set (mode_bytes[MOTBDIR]);
- IAm = mode_bytes[ID];
- rpm2mph = 60.0 // to Motor Revs per hour;
- * ((double)mode_bytes[MOTPIN] / (double)mode_bytes[WHEELGEAR]) // Wheel revs per hour
- * PI * ((double)mode_bytes[WHEELDIA] / 1000.0) // metres per hour
- * 39.37 / (1760.0 * 36.0); // miles per hour
-// }
- // Alternative ID 1 to 9
-// com2.printf ("Alternative ID = 0x%2x\r\n", buff[6]);
- }
+ MotorA.direction_set (mode.rd(MOTADIR)); // modes all setup in class eeprom_settings {} mode ; constructor
+ MotorB.direction_set (mode.rd(MOTBDIR));
+ MotorA.poles (mode.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise
+ MotorB.poles (mode.rd(MOTBPOLES)); // Only two calls are here
+ MotorA.set_mode (MOTOR_REGENBRAKE);
+ MotorB.set_mode (MOTOR_REGENBRAKE);
+ setVI (0.9, 0.5); // Power up with moderate regen braking applied
+
// T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
T2 = 0; // T2, T3, T4 As yet unused pins
// T3 = 0;
// T4 = 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);
- setVI (0.9, 0.5);
- if ((last_temp_count > 160) && (last_temp_count < 2400)) // in range -40 to +100 degree C
+ if ((last_temperature_count > 160) && (last_temperature_count < 2400)) // in range -40 to +100 degree C
temp_sensor_exists = true;
-// pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
+#ifdef USING_DC_MOTORS
dc_motor_kicker_timer.start ();
+#endif
int old_hand_controller_direction = T5;
- if (mode_bytes[COMM_SRC] == 3) { // Read fwd/rev switch 'T5', PA15 on 401
+ if (mode.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401
pc.printf ("Pot control\r\n");
if (T5)
- mode_set_both_motors (FORWARD, 0.0); // sets both motors
+ mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors
else
- mode_set_both_motors (REVERSE, 0.0);
+ mode_set_both_motors (MOTOR_REVERSE, 0.0);
}
- pc.printf ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]);
-
+ pc.printf ("About to start %s!, mode_bytes[COMM_SRC]= %d\r\n", const_version_string, mode.rd(COMM_SRC));
+ pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true" : "false");
+// pc.printf ("SystemCoreClock=%d, MAX_PWM_TICKS=%d\r\n", SystemCoreClock, MAX_PWM_TICKS);
+// pcc.test () ;
+// tsc.test () ;
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_pc () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
- command_line_interpreter_loco () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
+ pcc.core () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true
+ tsc.core () ; // 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
} // 32Hz original setting for loop repeat rate
loop_flag = false; // Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms
@@ -596,7 +525,7 @@
RC_chan_1.validate_rx();
RC_chan_2.validate_rx();
- switch (mode_bytes[COMM_SRC]) { // Look to selected source of driving command, act on commands from wherever
+ switch (mode.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever
case 0: // Invalid
break;
case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter
@@ -612,27 +541,33 @@
case RC_IN2: // RC_chan_2
break;
default:
- pc.printf ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]); // set error flag instead here
+ if (ESC_Error.read(FAULT_UNRECOGNISED_STATE)) {
+ pc.printf ("Unrecognised state %d\r\n", mode.rd(COMM_SRC)); // set error flag instead here
+ ESC_Error.set (FAULT_UNRECOGNISED_STATE, 1);
+ }
break;
} // endof switch (mode_bytes[COMM_SRC]) {
+#ifdef USING_DC_MOTORS
dc_motor_kicker_timer.reset ();
- 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
+#endif
+ MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second
+ MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
+
+#ifdef USING_DC_MOTORS
if (MotorA.dc_motor) {
- MotorA.raw_V_pwm (1);
+// MotorA.raw_V_pwm (1);
MotorA.low_side_on ();
}
if (MotorB.dc_motor) {
- MotorB.raw_V_pwm (1);
+// MotorB.raw_V_pwm (1);
MotorB.low_side_on ();
}
if (MotorA.dc_motor || MotorB.dc_motor) {
// motors_restore.attach_us (&restorer, ttime);
motors_restore.attach_us (&restorer, 25);
}
+#endif
if (flag_8Hz) { // do slower stuff
flag_8Hz = false;
@@ -641,7 +576,8 @@
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
+// com2.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
+ pc.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this
} // End of dealing with WatchDog timer timeout
if (WatchDog < 0)
WatchDog = 0;
@@ -649,16 +585,13 @@
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 (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
- MotorB.current_calc ();
+ ESC_Error.report_any (false); // retain = false - reset error after reporting once
/* if (temp_sensor_exists) {
double tmprt = (double) last_temp_count;
tmprt /= 16.0;
tmprt -= 50.0;
pc.printf ("Temp %.2f\r\n", tmprt);
}*/
-// 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);
-// com2.printf ("V=%+.2f, Pot=%+.2f, HA %d, HB %d, IAmin %d, IAave %d, IAmax %d, IB %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);
}
#define SERVO_OUT_TEST
#ifdef SERVO_OUT_TEST