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 8:93203f473f6e, committed 2018-08-18
- Comitter:
- JonFreeman
- Date:
- Sat Aug 18 12:51:35 2018 +0000
- Parent:
- 7:6deaeace9a3e
- Child:
- 9:ac2412df01be
- Commit message:
- Work underway to drive brushed motors.; Code as supplied to Rob
Changed in this revision
--- a/DualBLS.h Sun Jun 17 06:59:37 2018 +0000
+++ b/DualBLS.h Sat Aug 18 12:51:35 2018 +0000
@@ -6,20 +6,23 @@
const int TIMEOUT_SECONDS = 30;
/* Please Do Not Alter these */
-const int VOLTAGE_READ_INTERVAL_US = 50, // Interrupts timed every 50 micro sec, runs around loop performing 1 A-D conversion per pass
+const int PWM_PRESECALER_DEFAULT = 5,
+ VOLTAGE_READ_INTERVAL_US = 50, // Interrupts timed every 50 micro sec, runs around loop performing 1 A-D conversion per pass
MAIN_LOOP_REPEAT_TIME_US = 31250, // 31250 us, with TACHO_TAB_SIZE = 32 means tacho_ticks_per_time is tacho_ticks_per_second
MAIN_LOOP_ITERATION_Hz = 1000000 / MAIN_LOOP_REPEAT_TIME_US,
CURRENT_SAMPLES_AVERAGED = 100, // Current is spikey. Reading smoothed by using average of this many latest current readings
- PWM_HZ = 16000, // chosen to be above cutoff frequency of average human ear
- MAX_PWM_TICKS = (SystemCoreClock / PWM_HZ),
+ PWM_HZ = 15000, // chosen to be above cutoff frequency of average human ear
+ MAX_PWM_TICKS = (SystemCoreClock / (PWM_HZ * PWM_PRESECALER_DEFAULT)),
TICKLE_TIMES = 100 ,
WATCHDOG_RELOAD = (TIMEOUT_SECONDS * 8); // WatchDog counter ticked down in 8Hz loop
/* End of Please Do Not Alter these */
const double PI = 4.0 * atan(1.0),
TWOPI = 8.0 * atan(1.0);
+const double Pot_Brake_Range = 0.33;
-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} ; // 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
@@ -32,9 +35,15 @@
{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
- {20, 253, 98, "Wheel diameter mm"}, // New 01/06/2018
+ {50, 253, 98, "Wheel diameter mm"}, // New 01/06/2018
{10, 253, 27, "Motor pinion"}, // New 01/06/2018
- {20, 253, 85, "Wheel gear"}, // 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);
--- a/F401RE.h Sun Jun 17 06:59:37 2018 +0000 +++ b/F401RE.h Sat Aug 18 12:51:35 2018 +0000 @@ -91,8 +91,8 @@ DigitalOut T3 (PB_15); // Pin 36 // BufferedSerial com2 pins 37 Tx, 38 Rx BufferedSerial com2 (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module -FastPWM A_MAX_V_PWM (PC_8, 1), // Pin 39 pwm3/3 - A_MAX_I_PWM (PC_9, 1); // pin 40, prescaler value pwm3/4 +FastPWM A_MAX_V_PWM (PC_8, PWM_PRESECALER_DEFAULT), // Pin 39 pwm3/3 + A_MAX_I_PWM (PC_9, PWM_PRESECALER_DEFAULT); // pin 40, prescaler value pwm3/4 //InterruptIn MotB_Hall (PA_8); // Pin 41 // Pin 41 Port_A AWH // BufferedSerial com3 pins 42 Tx, 43 Rx @@ -126,8 +126,8 @@ InterruptIn MAH3 (PC_12); // Pin 53 InterruptIn MBH1 (PD_2); // Pin 54 DigitalOut T6 (PB_3); // Pin 55 -FastPWM B_MAX_V_PWM (PB_4, 1), // Pin 56 pwm3/3 - B_MAX_I_PWM (PB_5, 1); // pin 57, prescaler value pwm3/4 +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 // Pin 60 BOOT0
--- a/F446ZE.h Sun Jun 17 06:59:37 2018 +0000 +++ b/F446ZE.h Sat Aug 18 12:51:35 2018 +0000 @@ -1,16 +1,14 @@ // 5TH JUNE 2018 NONE OF THIS IS RIGHT YET !! - // Hoped to select servo functions from user info stored on EEROM. Too difficult. Do not define servo as in and out - // Port A -> MotorA, Port B -> MotorB const uint16_t -AUL = 1 << 0, // Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) - GOOD, works well with auto-tickle of high side drivers -AVL = 1 << 6, // These are which port bits connect to which mosfet driver -AWL = 1 << 4, +AUL = 1 << 1, // Feb 2018 Now using DGD21032 mosfet drivers via 74HC00 pwm gates (low side) - GOOD, works well with auto-tickle of high side drivers +AVL = 1 << 2, // These are which port bits connect to which mosfet driver +AWL = 1 << 3, -AUH = 1 << 1, -AVH = 1 << 7, -AWH = 1 << 8, +AUH = 1 << 4, +AVH = 1 << 5, +AWH = 1 << 6, AUV = AUH | AVL, // Each of 6 possible output energisations made up of one hi and one low AVU = AVH | AUL, @@ -29,8 +27,8 @@ BWL = 1 << 2, BUH = 1 << 10, -BVH = 1 << 12, -BWH = 1 << 13, +BVH = 1 << 11, +BWH = 1 << 3, BUV = BUH | BVL, BVU = BVH | BUL, @@ -46,14 +44,14 @@ PORT_A_MASK = AUL | AVL | AWL | AUH | AVH | AWH, // NEW METHOD FOR DGD21032 MOSFET DRIVERS PORT_B_MASK = BUL | BVL | BWL | BUH | BVH | BWH; - -PortOut MotA (PortA, PORT_A_MASK); // Activate output ports to motor drivers -PortOut MotB (PortB, PORT_B_MASK); +// Use Ports E and G - best first guess July 2018 +PortOut MotA (PortE, PORT_A_MASK); // Activate output ports to motor drivers +PortOut MotB (PortG, PORT_B_MASK); // Pin 1 VBAT NET +3V3 //DigitalIn J3 (PC_13, PullUp);// Pin 2 Jumper pulls to GND, R floats Hi -InterruptIn Temperature_pin (PC_13);// Pin 2 June 2018 - taken for temperature sensor - hard wired to T1 due to wrong thought T1 could be InterruptIn +InterruptIn Temperature_pin (PD_7);// Pin ?? June 2018 - taken for temperature sensor - hard wired to T1 due to wrong thought T1 could be InterruptIn // Pin 3 PC14-OSC32_IN NET O32I @@ -70,16 +68,16 @@ // 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 (PD_8, PD_9, 512, 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 // Pin 21 DigitalOut led1(LED1); -DigitalOut LED (PA_5); // Pin 21 +DigitalOut LED (PB_0); // Pin ?? as fitted to Nucleo board. Other leds use PB_7 and PB_14 // Pin 22 Port_A AVL // Pin 23 Port_A AVH -InterruptIn MBH2 (PC_4); // Pin 24 -InterruptIn MBH3 (PC_5); // Pin 25 +InterruptIn MBH2 (PD_1); // Pin ?? +InterruptIn MBH3 (PD_3); // Pin ?? // Pin 26 Port_B BUL // Pin 27 Port_B BVL // Pin 28 Port_B BWL @@ -89,17 +87,21 @@ // Pin 32 VDD // Pin 33 Port_B BVH // Pin 34 Port_B BWH -DigitalOut T4 (PB_14); // Pin 35 -DigitalOut T3 (PB_15); // Pin 36 + +//DigitalOut T4 (PB_14); // Pin ** +//DigitalOut T3 (PB_15); // Pin ** + // BufferedSerial com2 pins 37 Tx, 38 Rx -BufferedSerial com2 (PC_6, PC_7); // Pins 37, 38 tx, rx to XBee module -FastPWM A_MAX_V_PWM (PC_8, 1), // Pin 39 pwm3/3 - A_MAX_I_PWM (PC_9, 1); // pin 40, prescaler value pwm3/4 +BufferedSerial com2 (PC_10, PC_11); // Pins 37, 38 tx, rx to XBee module +FastPWM A_MAX_V_PWM (PC_8, PWM_PRESECALER_DEFAULT), // Pin 39 pwm3/3 + A_MAX_I_PWM (PC_9, PWM_PRESECALER_DEFAULT); // pin 40, prescaler value pwm3/4 //InterruptIn MotB_Hall (PA_8); // Pin 41 // Pin 41 Port_A AWH // BufferedSerial com3 pins 42 Tx, 43 Rx //InterruptIn tryseredge (PA_9); -BufferedSerial com3 (PA_9, PA_10); // Pins 42, 43 tx, rx to any aux module + +BufferedSerial com3 (PC_12, PD_2); // Pins 42, 43 tx, rx to any aux module + // PA_9 is Tx. I wonder, can we also use InterruptIn on this pin to generate interrupts on tx bit transitions ? Let's find out ! // No. @@ -111,7 +113,7 @@ //InterruptIn T1 (PA_12); // Pin 45 now input counting pulses from LMT01 temperature sensor // InterruptIn DOES NOT WORK ON PA_12. Boards are being made, will have to wire link PA12 to PC13 -DigitalIn T1 (PA_12); +//DigitalIn T1 (PA_12); ////InterruptIn T1 (PC_13); // Pin 45 now input counting pulses from LMT01 temperature sensor @@ -123,21 +125,21 @@ //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 -InterruptIn MBH1 (PD_2); // Pin 54 +InterruptIn MAH1 (PD_4); // Pin 51 +InterruptIn MAH2 (PD_5); // Pin 52 +InterruptIn MAH3 (PD_6); // Pin 53 +InterruptIn MBH1 (PD_0); // Pin 54 DigitalOut T6 (PB_3); // Pin 55 -FastPWM B_MAX_V_PWM (PB_4, 1), // Pin 56 pwm3/3 - B_MAX_I_PWM (PB_5, 1); // pin 57, prescaler value pwm3/4 +FastPWM B_MAX_V_PWM (PE_13, PWM_PRESECALER_DEFAULT), // Pin 56 pwm3/3 + B_MAX_I_PWM (PE_14, PWM_PRESECALER_DEFAULT); // pin 57, prescaler value pwm3/4 -I2C i2c (PB_7, PB_6); // Pins 58, 59 For 24LC64 eeprom +I2C i2c (PB_9, PB_8); // Pins 58, 59 For 24LC64 eeprom // Pin 60 BOOT0 // Servo pins, 2 off. Configured as Input to read radio control receiver // If used as servo output, code gives pin to 'Servo' - seems to work -InterruptIn Servo1_i (PB_8); // Pin 61 to read output from rc rx -InterruptIn Servo2_i (PB_9); // Pin 62 to read output from rc rx +InterruptIn Servo1_i (PF_14); // Pin 61 to read output from rc rx +InterruptIn Servo2_i (PF_15); // Pin 62 to read output from rc rx // Pin 63 VSS // Pin 64 VDD
--- a/cli_BLS_nortos.cpp Sun Jun 17 06:59:37 2018 +0000
+++ b/cli_BLS_nortos.cpp Sat Aug 18 12:51:35 2018 +0000
@@ -7,7 +7,9 @@
using namespace std;
extern int I_Am () ; // Returns boards id number as ASCII char '0', '1' etc. Code for Broadcast = '\r'
-extern int WatchDog;
+extern int WatchDog;
+extern bool WatchDogEnable;
+extern char mode_bytes[];
const int BROADCAST = '\r';
const int MAX_PARAMS = 20;
@@ -31,6 +33,11 @@
extern void read_last_VI (double * val) ; // only for test from cli
//BufferedSerial * com;
+extern double Read_DriverPot ();
+void pot_cmd (struct parameters & a)
+{
+ pc.printf ("Driver's pot %.3f\r\n", Read_DriverPot ());
+}
void null_cmd (struct parameters & a)
{
@@ -38,7 +45,45 @@
a.com->printf ("At null_cmd, board ID %c, parameters : First %.3f, second %.3f\r\n", I_Am(), a.dbl[0], a.dbl[1]);
}
-extern void mode_set (int mode, double val) ; // called from cli to set fw, re, rb, hb
+// {"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");
+}
+
+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 prscfuck (int);
+void pf_cmd (struct parameters & a)
+{
+ prscfuck ((int)a.dbl[0]);
+}
+
+extern void report_motor_types () ;
+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]);
+}
+
+extern void mode_set_both_motors (int mode, double val) ; // called from cli to set fw, re, rb, hb
extern void read_supply_vi (double * val) ;
void rdi_cmd (struct parameters & a) // read motor currents
@@ -61,19 +106,19 @@
void fw_cmd (struct parameters & a) // Forward command
{
- mode_set (FORWARD, 0.0);
+ mode_set_both_motors (FORWARD, 0.0);
}
void re_cmd (struct parameters & a) // Reverse command
{
- mode_set (REVERSE, 0.0);
+ 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 (REGENBRAKE, b);
+ mode_set_both_motors (REGENBRAKE, b);
// apply_brake (b);
}
@@ -105,7 +150,25 @@
// last;
} ;
*/
-extern char mode_bytes[];
+
+// 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');
+ }
+}
void mode_cmd (struct parameters & a) // With no params, reads eeprom contents. With params sets eeprom contents
{
@@ -154,7 +217,7 @@
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 (HANDBRAKE, 0.0);
+ mode_set_both_motors (HANDBRAKE, 0.0);
}
extern uint32_t last_temp_count;
@@ -183,9 +246,11 @@
extern double rpm2mph ;
void mph_cmd (struct parameters & a) // to report miles per hour
{
+ if (a.respond) {
uint32_t dest[3];
read_RPM (dest); // gets rpm for each motor
- a.com->printf ("mph%c %.3f\r\n", mode_bytes[ID], (double)(dest[0] + dest[1]) * rpm2mph / 2.0);
+ a.com->printf ("mph%c %.3f\r", mode_bytes[ID], (double)(dest[0] + dest[1]) * rpm2mph / 2.0);
+ }
}
void menucmd (struct parameters & a);
@@ -240,15 +305,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},
- {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
+ {"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},
+ {"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},
};
@@ -258,6 +327,9 @@
struct kb_command const pc_command_list[] = {
{"ls", "Lists available commands", menucmd},
{"?", "Lists available commands, same as ls", menucmd},
+ {"mtypes", "report types of motors found", mt_cmd},
+ {"pf", "try changing FastPWM prescaler values", pf_cmd},
+ {"pot", "read drivers control pot", pot_cmd},
{"fw", "forward", fw_cmd},
{"re", "reverse", re_cmd},
{"rb", "regen brake 0 to 99 %", rb_cmd},
@@ -265,16 +337,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},
- {"who", "search for connected units, e.g. 3who returs 'Hi there' if found", who_cmd},
+ {"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_pccmd},
+ {"wddi", "disable watchdog always", wddi_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'
{"nu", "do nothing", null_cmd},
};
--- a/main.cpp Sun Jun 17 06:59:37 2018 +0000
+++ b/main.cpp Sat Aug 18 12:51:35 2018 +0000
@@ -1,4 +1,6 @@
#include "mbed.h"
+//#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
+#include "stm32f401xe.h"
#include "DualBLS.h"
#include "BufferedSerial.h"
#include "FastPWM.h"
@@ -28,6 +30,7 @@
*/
+
#if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP
#include "F401RE.h"
#endif
@@ -37,6 +40,7 @@
/* Global variable declarations */
volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
+bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable
uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts
driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot
sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US
@@ -51,17 +55,20 @@
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
+// struct single_bogie_options bogie;
+double rpm2mph = 0.0; // gets calculated from eeprom mode entries if present
/* 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;
+Timer dc_motor_kicker_timer;
+Timeout motors_restore;
// Interrupt Service Routines
-void ISR_temperature_find_ticker () { // every 960 us, i.e. slightly faster than once per milli sec
+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)) {
@@ -100,7 +107,8 @@
class RControl_In
-{ // Read servo style pwm input
+{
+ // Read servo style pwm input
Timer t;
int32_t clock_old, clock_new;
int32_t pulse_width_us, period_us;
@@ -117,11 +125,13 @@
bool rx_active;
} ;
-uint32_t RControl_In::pulsewidth () {
+uint32_t RControl_In::pulsewidth ()
+{
return pulse_width_us;
}
-uint32_t RControl_In::period () {
+uint32_t RControl_In::period ()
+{
return period_us;
}
@@ -162,12 +172,19 @@
4 1 0 0 0 1 1 -___---___-- Hall3
UV WV WU VU VW UW OUTPUT SEQUENCE
+
+8th July 2018
+Added drive to DC brushed motors.
+Connect U and W to dc motor, leave V open.
+
+Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. Use this for dc motors
+
*/
const uint16_t A_tabl[] = { // Origial table
0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
- 0, AWV,AVU,AWU,AUW,AUV,AVW, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
- 0, AVW,AUV,AUW,AWU,AVU,AWV, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
- 0, BRA,BRA,BRA,BRA,BRA,BRA,0, // Regenerative Braking
+ 0, AWV,AVU,AWU,AUW,AUV,AVW,AUW, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
+ 0, AVW,AUV,AUW,AWU,AVU,AWV,AWU, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
+ 0, BRA,BRA,BRA,BRA,BRA,BRA,BRA, // Regenerative Braking
KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
} ;
InterruptIn * AHarr[] = {
@@ -177,9 +194,9 @@
} ;
const uint16_t B_tabl[] = {
0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
- 0, BWV,BVU,BWU,BUW,BUV,BVW, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
- 0, BVW,BUV,BUW,BWU,BVU,BWV, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
- 0, BRB,BRB,BRB,BRB,BRB,BRB,0, // Regenerative Braking
+ 0, BWV,BVU,BWU,BUW,BUV,BVW,BUW, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
+ 0, BVW,BUV,BUW,BWU,BVU,BWV,BWU, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
+ 0, BRB,BRB,BRB,BRB,BRB,BRB,BRB, // Regenerative Braking
KEEP_L_MASK_B, KEEP_H_MASK_B
} ;
InterruptIn * BHarr[] = {
@@ -198,6 +215,7 @@
PortOut * Motor_Port;
InterruptIn * Hall1, * Hall2, * Hall3;
public:
+ bool dc_motor;
struct currents {
uint32_t max, min, ave;
} I;
@@ -218,7 +236,10 @@
void current_calc () ; // Updates 3 uint32_t I.min, I.ave, I.max
uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec
int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs
+ bool motor_is_brushless ();
void high_side_off () ;
+ void low_side_on () ;
+ void raw_V_pwm (int);
} ; //MotorA, MotorB, or even Motor[2];
motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr);
@@ -227,7 +248,8 @@
//motor * MotPtr[8]; // Array of pointers to some number of motor objects
motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor
-{ // Constructor
+{
+ // Constructor
maxV = _maxV_;
maxI = _maxI_;
Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
@@ -258,19 +280,36 @@
PPS = 0;
RPM = 0;
last_V = last_I = 0.0;
+ int x = read_Halls ();
+ if (x == 7)
+ dc_motor = true;
+ else
+ dc_motor = false;
+}
+
+bool motor::motor_is_brushless ()
+{
+ /* int x = read_Halls ();
+ if (x < 1 || x > 6)
+ return false;
+ return true;
+ */
+ return !dc_motor;
}
/**
void motor::direction_set (int dir) {
Used to set direction according to mode data from eeprom
*/
-void motor::direction_set (int dir) {
+void motor::direction_set (int dir)
+{
if (dir != 0)
dir = FORWARD | REVERSE; // bits used in eor
direction = dir;
}
-int motor::read_Halls () {
+int motor::read_Halls ()
+{
int x = 0;
if (*Hall1 != 0) x |= 1;
if (*Hall2 != 0) x |= 2;
@@ -278,12 +317,20 @@
return x;
}
-void motor::high_side_off () {
+void motor::high_side_off ()
+{
uint16_t p = *Motor_Port;
p &= lut[32]; // KEEP_L_MASK_A or B
*Motor_Port = p;
}
+void motor::low_side_on ()
+{
+// uint16_t p = *Motor_Port;
+// p &= lut[31]; // KEEP_L_MASK_A or B
+ *Motor_Port = lut[31];
+}
+
void motor::current_calc ()
{
I.min = 0x0fffffff; // samples are 16 bit
@@ -301,7 +348,15 @@
I.ave /= CURRENT_SAMPLES_AVERAGED;
}
-void motor::set_V_limit (double p) // Sets max motor voltage
+
+void 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 motor::set_V_limit (double p) // Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
{
if (p < 0.0)
p = 0.0;
@@ -330,12 +385,15 @@
}
uint32_t motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
-{ // Can also test for motor running or not here
+{
+ // Can also test for motor running or not here
+ 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 {
+ } else {
moving_flag = true;
ppstmp = Hall_total;
}
@@ -378,17 +436,16 @@
void motor::Hall_change ()
{
- const int32_t delta_theta_lut[] = // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
- {
- 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
- 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
- 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
- 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
- 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
- 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
- 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
- 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
- } ;
+ const int32_t delta_theta_lut[] = { // Looks up -1 for forward move detected, +1 for reverse move detected, 0 for error or unknown
+ 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 0
+ 0, 0, 0,-1, 0, 1, 0, 0, // Previous Hindex was 1
+ 0, 0, 0, 1, 0, 0,-1, 0, // Previous Hindex was 2
+ 0, 1,-1, 0, 0, 0, 0, 0, // Previous Hindex was 3
+ 0, 0, 0, 0, 0,-1, 1, 0, // Previous Hindex was 4
+ 0,-1, 0, 0, 1, 0, 0, 0, // Previous Hindex was 5
+ 0, 0, 1, 0,-1, 0, 0, 0, // Previous Hindex was 6
+ 0, 0, 0, 0, 0, 0, 0, 0, // Previous Hindex was 7
+ } ;
int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
if (delta_theta == 0)
encoder_error_cnt++;
@@ -400,7 +457,13 @@
}
-void temp_sensor_isr () { // got rising edge from LMT01. ALMOST CERTAIN this misses some
+void report_motor_types () // not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise
+{
+ pc.printf ("Mot A is %s, Mot B is %s\r\n", MotorA.motor_is_brushless() ? "Brushless":"DC", MotorB.motor_is_brushless() ? "Brushless":"DC");
+}
+
+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++;
@@ -409,11 +472,12 @@
// T2 = !T2; // scope hanger
}
+
void MAH_isr ()
{
uint32_t x = 0;
MotorA.high_side_off ();
- T3 = !T3;
+// T3 = !T3;
if (MAH1 != 0) x |= 1;
if (MAH2 != 0) x |= 2;
if (MAH3 != 0) x |= 4;
@@ -441,32 +505,38 @@
*Motor_Port = lut[inner_mode | Hindex[0]];
}
-void setVI (double v, double i) {
+void setVI (double v, double i)
+{
MotorA.set_V_limit (v); // Sets max motor voltage
MotorA.set_I_limit (i); // Sets max motor current
MotorB.set_V_limit (v);
MotorB.set_I_limit (i);
}
-void setV (double v) {
+void setV (double v)
+{
MotorA.set_V_limit (v);
MotorB.set_V_limit (v);
}
-void setI (double i) {
+void setI (double i)
+{
MotorA.set_I_limit (i);
MotorB.set_I_limit (i);
}
-void read_RPM (uint32_t * dest) {
+void read_RPM (uint32_t * dest)
+{
dest[0] = MotorA.RPM;
dest[1] = MotorB.RPM;
}
-void read_PPS (uint32_t * dest) {
+void read_PPS (uint32_t * dest)
+{
dest[0] = MotorA.PPS;
dest[1] = MotorB.PPS;
}
-void read_last_VI (double * d) { // only for test from cli
+void read_last_VI (double * d) // only for test from cli
+{
d[0] = MotorA.last_V;
d[1] = MotorA.last_I;
d[2] = MotorB.last_V;
@@ -481,13 +551,22 @@
void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
{
static uint32_t i = 0, tab_ptr = 0;
-
+// 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 - silly semaphore count %d, limiting to sensible\r\n", AtoD_Semaphore);
+ pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
AtoD_Semaphore = 20;
}
while (AtoD_Semaphore > 0) {
@@ -516,10 +595,12 @@
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 ();
}
@@ -540,13 +621,15 @@
return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
}
-void read_supply_vi (double * val) { // called from cli
+void read_supply_vi (double * val) // called from cli
+{
val[0] = MotorA.I.ave;
val[1] = MotorB.I.ave;
val[2] = Read_BatteryVolts ();
}
-void mode_set (int mode, double val) { // called from cli to set fw, re, rb, hb
+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) {
@@ -577,17 +660,140 @@
last;
} ;
*/
-int I_Am () { // Returns boards id number as ASCII char
+int I_Am () // Returns boards id number as ASCII char
+{
return IAm;
}
-double mph (int rpm) {
+double mph (int rpm)
+{
if (mode_good_flag) {
return rpm2mph * (double) rpm;
}
return -1.0;
}
+void restorer ()
+{
+ motors_restore.detach ();
+ if (MotorA.dc_motor) {
+ MotorA.set_V_limit (MotorA.last_V);
+ MotorA.set_I_limit (MotorA.last_I);
+ MotorA.motor_set ();
+ }
+ if (MotorB.dc_motor) {
+ MotorB.set_V_limit (MotorB.last_V);
+ MotorB.set_I_limit (MotorB.last_I);
+ MotorB.motor_set ();
+ }
+}
+
+//int ttime = 3; // resettable via cli 'stt', used to determine suitable force low on period for driving dc motor
+bool worth_the_bother (double a, double b) {
+ double c = a - b;
+ if (c < 0.0)
+ c = 0.0 - c;
+ if (c > 0.02) {
+// pc.printf ("%.3f\r\n", c);
+ return true;
+ }
+ return false;
+}
+
+void prscfuck (int v) {
+/*
+int prescaler ( int value )
+
+Set the PWM prescaler.
+
+The period of all PWM pins on the same PWM unit have to be reset after using this!
+
+Parameters:
+ value - The required prescaler. Special values: 0 = lock current prescaler, -1 = use dynamic prescaler
+ return - The prescaler which was set (can differ from requested prescaler if not possible)
+
+Definition at line 82 of file FastPWM_common.cpp.
+*/
+ if (v < 1)
+ v = 1;
+ int w = A_MAX_V_PWM.prescaler (v);
+ pc.printf ("Attempt setting prescaler %d returned %d\r\n", v, w);
+}
+
+enum { HAND_CONT_STATE_BEGIN,
+ HAND_CONT_STATE_POWER_CYCLE_TO_EXIT,
+ HAND_CONT_STATE_INTO_BRAKING,
+ HAND_CONT_STATE_INTO_DRIVING,
+ HAND_CONT_STATE_BRAKING,
+ HAND_CONT_STATE_DRIVING
+ } ;
+
+void hand_control_state_machine () {
+ static int new_hand_controller_state = HAND_CONT_STATE_BEGIN;
+// static int old_hand_controller_state = HAND_CONT_STATE_BEGIN;
+ static int old_hand_controller_direction = T5, t = 0;
+ static double brake_effort, drive_effort, pot_position, old_pot_position = 0.0;
+ if (T5 != old_hand_controller_direction) { // 1 Forward, 0 Reverse
+ pc.printf ("Direction change! Power off then on again to resume\r\n");
+ mode_set_both_motors (REGENBRAKE, 1.0);
+// old_hand_controller_state = new_hand_controller_state;
+ new_hand_controller_state = HAND_CONT_STATE_POWER_CYCLE_TO_EXIT;
+ }
+ pot_position = Read_DriverPot (); // gets to here on first pass before pot has been read
+ switch (new_hand_controller_state) {
+ case HAND_CONT_STATE_BEGIN:
+ pot_position = Read_DriverPot ();
+ if (t++ > 2 && pot_position < 0.02) {
+ pc.printf ("In BEGIN, pot %.2f\r\n", pot_position);
+ new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
+ }
+ break;
+ case HAND_CONT_STATE_POWER_CYCLE_TO_EXIT:
+ break;
+ case HAND_CONT_STATE_INTO_BRAKING:
+ brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
+ mode_set_both_motors (REGENBRAKE, brake_effort);
+ old_pot_position = pot_position;
+ new_hand_controller_state = HAND_CONT_STATE_BRAKING;
+ pc.printf ("Brake\r\n");
+ break;
+ case HAND_CONT_STATE_INTO_DRIVING:
+ new_hand_controller_state = HAND_CONT_STATE_DRIVING;
+ pc.printf ("Drive\r\n");
+ if (T5)
+ mode_set_both_motors (FORWARD, 0.0); // sets both motors
+ else
+ mode_set_both_motors (REVERSE, 0.0);
+ break;
+ case HAND_CONT_STATE_BRAKING:
+ if (pot_position > Pot_Brake_Range) // escape braking into drive
+ new_hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
+ else {
+ 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);
+// pc.printf ("Brake %.2f\r\n", brake_effort);
+ }
+ }
+ break;
+ case HAND_CONT_STATE_DRIVING:
+ if (pot_position < Pot_Brake_Range) // escape braking into drive
+ new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
+ else {
+ if (worth_the_bother(pot_position, old_pot_position)) {
+ old_pot_position = pot_position;
+ drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range);
+ setVI (drive_effort, drive_effort); // Wind volts and amps up and down together ???
+ pc.printf ("Drive %.2f\r\n", drive_effort);
+ }
+ }
+ break;
+ default:
+ break;
+ } // endof switch (hand_controller_state) {
+}
+
int main()
{
int eighth_sec_count = 0;
@@ -596,7 +802,7 @@
MotB = 0;
// MotPtr[0] = &MotorA; // Pointers to motor class objects
// MotPtr[1] = &MotorB;
-
+
Temperature_pin.fall (&temp_sensor_isr);
Temperature_pin.mode (PullUp);
@@ -642,41 +848,47 @@
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
+ } 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");
- int err = 0;
+// 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);
- err++;
+ 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 (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
+ 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]);
}
// 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;
+// T3 = 0;
+// T4 = 0;
// T5 = 0; now input from fw/re on remote control box
T6 = 0;
// MotPtr[0]->set_mode (REGENBRAKE);
@@ -684,6 +896,8 @@
MotorB.set_mode (REGENBRAKE);
setVI (0.9, 0.5);
+// prscfuck (PWM_PRESECALER_DEFAULT); // Test fucking with fastpwm prescaler
+
Servos[0] = Servos[1] = NULL;
// NOTE The ONLY way to get both servos working properly is to NOT use any if (bla) Servo ervo1(PB_8);
// Only works with unconditional inline code
@@ -693,17 +907,17 @@
Servos[0] = & Servo1;
Servo Servo2 (PB_9) ;
Servos[1] = & Servo2;
-
+
// pc.printf ("last_temp_count = %d\r\n", last_temp_count); // Has had time to do at least 1 conversion
if ((last_temp_count > 160) && (last_temp_count < 2400)) // in range -40 to +100 degree C
temp_sensor_exists = true;
-/*
- // Setup Complete ! Can now start main control forever loop.
- // March 16th 2018 thoughts !!!
- // Control from one of several sources and types as selected in options bytes in eeprom.
- // Control could be from e.g. Pot, Com2, Servos etc.
- // Suggest e.g.
-*/ /*
+ /*
+ // Setup Complete ! Can now start main control forever loop.
+ // March 16th 2018 thoughts !!!
+ // Control from one of several sources and types as selected in options bytes in eeprom.
+ // Control could be from e.g. Pot, Com2, Servos etc.
+ // Suggest e.g.
+ */ /*
switch (mode_byte) { // executed once only upon startup
case POT:
while (1) { // forever loop
@@ -720,7 +934,18 @@
}
*/
// pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
- pc.printf ("About to start!\r\n");
+ dc_motor_kicker_timer.start ();
+ int old_hand_controller_direction = T5;
+ if (mode_bytes[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
+ else
+ mode_set_both_motors (REVERSE, 0.0);
+ }
+
+ pc.printf ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]);
+
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
@@ -728,31 +953,66 @@
AtoD_reader (); // Performs A to D conversions at rate set by ticker interrupts
}
loop_flag = false; // Clear flag set by ticker interrupt handler
+ switch (mode_bytes[COMM_SRC]) { // Look to selected source of driving command, act on commands from wherever
+ case 0: // Invalid
+ break;
+ case 1: // COM1 - Primarily for testing, bypassed by command line interpreter
+ break;
+ case 2: // COM2 - Primarily for testing, bypassed by command line interpreter
+ break;
+ case 3: // Put all hand controller input stuff here
+ hand_control_state_machine ();
+ break; // endof hand controller stuff
+ case 4: // Servo1
+ break;
+ case 5: // Servo2
+ break;
+ default:
+ pc.printf ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]);
+ break;
+ } // endof switch (mode_bytes[COMM_SRC]) {
+
+ 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
+// T4 = !T4; // toggle to hang scope on to verify loop execution
// do stuff
+ if (MotorA.dc_motor) {
+ MotorA.raw_V_pwm (1);
+ MotorA.low_side_on ();
+ }
+ if (MotorB.dc_motor) {
+ 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);
+ }
+
if (flag_8Hz) { // do slower stuff
flag_8Hz = false;
LED = !LED; // Toggle LED on board, should be seen to fast flash
- WatchDog--;
- if (WatchDog == 0) { // Deal with WatchDog timer timeout here
- setVI (0.0, 0.0); // set motor volts and amps to zero
- com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
- } // End of dealing with WatchDog timer timeout
- if (WatchDog < 0)
- WatchDog = 0;
+ if (WatchDogEnable) {
+ WatchDog--;
+ if (WatchDog == 0) { // Deal with WatchDog timer timeout here
+ setVI (0.0, 0.0); // set motor volts and amps to zero
+ com2.printf ("TIMEOUT %2x\r\n", (I_Am() & 0x0f)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD
+ } // End of dealing with WatchDog timer timeout
+ if (WatchDog < 0)
+ WatchDog = 0;
+ }
eighth_sec_count++;
if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts
eighth_sec_count = 0;
MotorA.current_calc (); // Updates readings in MotorA.I.min, MotorA.I.ave and MotorA.I.max
MotorB.current_calc ();
-/* if (temp_sensor_exists) {
- double tmprt = (double) last_temp_count;
- tmprt /= 16.0;
- tmprt -= 50.0;
- pc.printf ("Temp %.2f\r\n", tmprt);
- }*/
+ /* 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);
}
} // End of if(flag_8Hz)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sud.cpp Sat Aug 18 12:51:35 2018 +0000
@@ -0,0 +1,117 @@
+//sudoku
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "DualBLS.h"
+extern BufferedSerial com2, pc;
+using namespace std;
+
+struct cell {
+ uint16_t confirmed;
+ uint16_t dominoes;
+ uint16_t must_be;
+} matrix[82];
+/*
+Cell Map
+
+0 1 2 3 4 5 6 7 8
+9 10 11 12 13 14 15 16 17
+18 19 20 21 22 23 24 25 26
+
+27 28 29 30 31 32 33 34 35
+36 37 38 39 40 41 42 43 44
+45 46 47 48 49 50 51 52 53
+
+54 55 56 57 58 59 60 61 62
+63 64 65 66 67 68 69 70 71
+72 73 74 75 76 77 78 79 80
+
+*/
+
+static const uint16_t
+ box1[] = { 0, 1, 2, 9, 10, 11, 18, 19, 20},
+ box2[] = { 3, 4, 5, 12, 13, 14, 21, 22, 23},
+ box3[] = { 6, 7, 8, 15, 16, 17, 24, 25, 26},
+ box4[] = {27, 28, 29, 36, 37, 38, 45, 46, 47},
+ box5[] = {30, 31, 32, 39, 40, 41, 48, 49, 50},
+ box6[] = {33, 34, 35, 42, 43, 44, 51, 52, 53},
+ box7[] = {54, 55, 56, 63, 64, 65, 72, 73, 74},
+ box8[] = {57, 58, 59, 66, 67, 68, 75, 76, 77},
+ box9[] = {60, 61, 62, 69, 70, 71, 78, 79, 80},
+
+ col1[] = { 0, 9, 18, 27, 36, 45, 54, 63, 72},
+ col2[] = { 1, 10, 19, 28, 37, 46, 55, 64, 73},
+ col3[] = { 2, 11, 20, 29, 38, 47, 56, 65, 74},
+ col4[] = { 3, 12, 21, 30, 39, 48, 57, 66, 75},
+ col5[] = { 4, 13, 22, 31, 40, 49, 58, 67, 76},
+ col6[] = { 5, 14, 23, 32, 41, 50, 59, 68, 77},
+ col7[] = { 6, 15, 24, 33, 42, 51, 60, 69, 78},
+ col8[] = { 7, 16, 25, 34, 43, 52, 61, 70, 79},
+ col9[] = { 8, 17, 26, 35, 44, 53, 62, 71, 80},
+
+ row1[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8},
+ row2[] = { 9, 10, 11, 12, 13, 14, 15, 16, 17},
+ row3[] = {18, 19, 20, 21, 22, 23, 24, 25, 26},
+ row4[] = {27, 28, 29, 30, 31, 32, 33, 34, 35},
+ row5[] = {36, 37, 38, 39, 40, 41, 42, 43, 44},
+ row6[] = {45, 46, 47, 48, 49, 50, 51, 52, 53},
+ row7[] = {54, 55, 56, 57, 58, 59, 60, 61, 62},
+ row8[] = {63, 64, 65, 66, 67, 68, 69, 70, 71},
+ row9[] = {72, 73, 74, 75, 76, 77, 78, 79, 80},
+
+ *box[] = {box1, box2, box3, box4, box5, box6, box7, box8, box9},
+ *row[] = {row1, row2, row3, row4, row5, row6, row7, row8, row9},
+ *col[] = {col1, col2, col3, col4, col5, col6, col7, col8, col9},
+
+ examp1[] = {5,0,0,0,9,0,7,0,0, // Hard puzzle 2,706,560,598
+ 0,0,8,2,0,0,5,3,0,
+ 0,6,0,8,0,0,0,0,0,
+ 0,0,0,3,7,9,0,0,2,
+ 0,2,0,0,0,0,0,9,0,
+ 3,0,0,5,8,2,0,0,0,
+ 0,0,0,0,0,6,0,8,0,
+ 0,7,4,0,0,8,3,0,0,
+ 0,0,6,0,1,0,0,0,7};
+
+void clear_matrix () {
+ for (int i = 0; i < 81; i++) {
+ matrix[i].confirmed = matrix[i].must_be = 0;
+ matrix[i].dominoes = 0x3fe;
+ }
+}
+void load_matrix (const uint16_t * p) {
+ for (int i = 0; i < 81; i++)
+ matrix[i].confirmed = p[i];
+}
+bool ranged (uint16_t a) {
+ if ((a > 0) && (a < 10))
+ return true;
+ return false;
+}
+void knock_down_dominoes (const uint16_t * brc) { // param points to list of 9 matrix offsets. May refer to row, col, or box
+ int d, mask;
+ for (int i = 0; i < 9; i++) {
+ d = matrix[brc[i]].confirmed;
+ pc.printf ("matrix cell %d\r\n", brc[i]);
+ if (ranged(d)) {
+ mask = (~(1 << d)) & 0x3fe;
+ for (int j = 0; j < 9; j++) {
+ if (i == j)
+ matrix[brc[j]].dominoes |= 1 << d;
+ else
+ matrix[brc[j]].dominoes &= mask;
+ }
+ }
+ }
+}
+void update_matrix () {
+ for (int i = 0; i < 9; i++) {
+ knock_down_dominoes (row[i]);
+ knock_down_dominoes (col[i]);
+ knock_down_dominoes (box[i]);
+ }
+}
+void sudo () {
+ clear_matrix ();
+ load_matrix (examp1);
+ pc.printf ("Row 3 col 7 = %d\r\n", matrix[row[3][7]].dominoes);
+}