Jon Freeman
Dual Brushless Motor ESC, 10-62V, up to 50A per motor. Motors ganged or independent, multiple control input methods, cycle-by-cycle current limit, speed mode and torque mode control. Motors tiny to kW. Speed limit and other parameters easily set in firmware. As used in 'The Brushless Brutalist' locomotive - www.jons-workshop.com. See also Model Engineer magazine June-October 2019.
Dependencies: mbed BufferedSerial Servo PCT2075 FastPWM
Update 17th August 2020 Radio control inputs completed
Diff: main.cpp
- Revision:
- 16:d1e4b9ad3b8b
- Parent:
- 15:2591e2008323
- Child:
- 17:cc9b854295d6
--- a/main.cpp Sat Nov 30 18:40:30 2019 +0000
+++ b/main.cpp Tue Jun 09 09:20:19 2020 +0000
@@ -1,7 +1,7 @@
/*
STM3_ESC Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control.
Jon Freeman B. Eng Hons
- 2015 - 2019
+ 2015 - 2020
*/
#include "mbed.h"
#include "STM3_ESC.h"
@@ -10,11 +10,13 @@
#include "Servo.h"
#include "brushless_motor.h"
#include "Radio_Control_In.h"
-//#ifdef TARGET_NUCLEO_F401RE //
-
+#include "LM75B.h" // New I2C temp sensor code March 2020 (to suit possible next board issue, harmless otherwise)
+//#ifdef TARGET_NUCLEO_F401RE // Target is TARGET_NUCLEO_F401RE for all boards produced.
//#endif
/*
Brushless_STM3_ESC board
+Apr 2020 * RC tested on 'Idle Halt' branch line - all good - also tested Inverter Gen power sorce. All good.
+Dec 2019 * Radio control inputs now fully implemented, requires fitting tiny 'RC_in_fix' 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
@@ -29,6 +31,7 @@
LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable
March 2019 temp sensor only included with TEMP_SENSOR_ENABLE defined. Temp reading not essential, LMT01 was not a good choice due to
significant loading of interrupts, threatening integrity of Real Time System
+ *** New sensor code for LM75B temp sensor added March 2020 ***
*/
@@ -39,7 +42,7 @@
____________________________________________________________________________________
CONTROL PHILOSOPHY
-This STM3_ESC Bogie drive board software should ensure sensible control when commands supplied are not sensible !
+This STM3_ESC Dual Brushless Motor drive board software should ensure sensible control when commands supplied are not sensible !
That is, smooth transition between Drive, Coast and Brake to be implemented here.
The remote controller must not be relied upon to deliver sensible command sequences.
@@ -51,63 +54,58 @@
*/
-#if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP
+//#if defined (TARGET_NUCLEO_L476RG) // CPU in 64 pin LQFP ** NOT PROVED ** No good, pinmap different
+//#include "F401RE.h"
+//#endif
+#if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP - This is what all produced boards have
#include "F401RE.h"
#endif
-#if defined (TARGET_NUCLEO_F411RE) // CPU in 64 pin LQFP
+#if defined (TARGET_NUCLEO_F411RE) // CPU in 64 pin LQFP - Never tried, but probably would work as is
#include "F411RE.h"
#endif
#if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP
#include "F446ZE.h" // A thought for future version
#endif
/* Global variable declarations */
-char const_version_string[] = {"1.0.y2019.m09.d29\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
-int WatchDog = 0; // Set this up in main once pre-flight checks done. 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
+
+//volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
+uint32_t WatchDog = 0, // Set this up in main once pre-flight checks done. Allow extra few seconds at powerup
+ 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, 31250us at Sept 2019
AtoD_Semaphore = 0;
+bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable
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;
-double rpm2mph;
+bool temp_sensor_exists = false; // March 2020 - Now used to indicate presence or not of LM75B i2c temp sensor
-double Current_Scaler_Sep_2019 = 1.0; // New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom.
- // See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I
- // at lower voltages. This is simply because current takes longer to build in motor inductance when voltage
- // is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning
- // similar current flows for shorter times at higher voltages.
-
+/*
+ * Not used since LMT01 proved not a good choice. See LM75B code added March 2020
+ *
#ifdef TEMP_SENSOR_ENABLE
uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01
last_temperature_count = 0; // global updated approx every 100ms after each LMT01 conversion completes
-#endif
-/* 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
-#ifdef TEMP_SENSOR_ENABLE
Ticker temperature_find_ticker;
Timer temperature_timer;
#endif
-#ifdef USING_DC_MOTORS
-Timer dc_motor_kicker_timer;
-Timeout motors_restore;
-#endif
+*/
+/* End of Global variable declarations */
-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
+Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc - 50 us
+Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop
+
+eeprom_settings user_settings (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup
+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
error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes.
-eeprom_settings mode (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup
+//PCT2075 temp_sensor( i2c ); // or LM75B temp_sensor( p?, p? ); Added March 2020
+PCT2075 temp_sensor( SDA_PIN, SCL_PIN ); // or LM75B temp_sensor( p?, p? ); Added March 2020
-//uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
+
/*
- 5 1 3 2 6 4 SENSOR SEQUENCE
+ 5 1 3 2 6 4 Brushless Motor Hall SENSOR SEQUENCE
1 1 1 1 0 0 0 ---___---___ Hall1
2 0 0 1 1 1 0 __---___---_ Hall2
@@ -115,36 +113,39 @@
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
+Dec 2019 Support for DC motors deleted.
+Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7.
*/
-const uint16_t A_tabl[] = { // Origial table
- 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
- 0, AWHVL,AVHUL,AWHUL,AUHWL,AUHVL,AVHWL,AUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
- 0, AVHWL,AUHVL,AUHWL,AWHUL,AVHUL,AWHVL,AWHUL, // 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
+const uint16_t A_tabl[] = { // Table of motor energisation bit patterns. Rows are Handbrake, Forward, Reverse, Regen brake. Cols relate to Hall sensor outputs
+// AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH
+ 0, AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH, 0, // Handbrake
+// 1->3 2->6 3->2 4->5 5->1 6->4
+ 0, AWHVL, AVHUL, AWHUL, AUHWL, AUHVL, AVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,
+// 1->5 2->3 3->1 4->6 5->4 6->2
+ 0, AVHWL, AUHVL, AUHWL, AWHUL, AVHUL, AWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,
+ 0, BRA, BRA, BRA, BRA, BRA, BRA, BRA, // Regenerative Braking
KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
} ;
-
-const uint16_t B_tabl[] = {
- 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
- 0, BWHVL,BVHUL,BWHUL,BUHWL,BUHVL,BVHWL,BUHWL, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
- 0, BVHWL,BUHVL,BUHWL,BWHUL,BVHUL,BWHVL,BWHUL, // 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
+// AUHVL|AWL, AWHUL|AVL, AWHVL|AUH, AVHWL|AUL, AUHWL|AVH, AVHUL|AWH
+const uint16_t B_tabl[] = { // Different tables for Motors A and B as different ports and different port bits used.
+// 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
+ 0, BUHVL|BWL, BWHUL|BVL, BWHVL|BUH, BVHWL|BUL, BUHWL|BVH, BVHUL|BWH, 0, // Handbrake
+ 0, BWHVL, BVHUL, BWHUL, BUHWL, BUHVL, BVHWL, 0, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,
+ 0, BVHWL, BUHVL, BUHWL, BWHUL, BVHUL, BWHVL, 0, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,
+ 0, BRB, BRB, BRB, BRB, BRB, BRB, BRB, // Regenerative Braking
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, ISHUNTA);
-brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB);
-
+brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB); // Two brushless motor instantiations
extern cli_2019 pcc, tsc; // command line interpreters from pc and touch screen
+static const int LM75_I2C_ADDR = 0x90; // i2c temperature sensor
+
// Interrupt Service Routines
+/*
#ifdef TEMP_SENSOR_ENABLE
void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec
{
@@ -158,13 +159,25 @@
if (t == 6)
readflag = false;
}
+
+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
+}
#endif
+*/
+
/** 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
+void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US (32Hz)
{
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
@@ -180,31 +193,41 @@
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
+// fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
+}
+
+// End of Interrupt Service Routines
+
+const char * get_version () {
+ return "1.0.y2020.m05.d21\0"; // Version string, readable using 'ver' serial command
}
-#ifdef TEMP_SENSOR_ENABLE
-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
+bool read_temperature (float & t) {
+// pc.printf ("test param temp = %7.3f\r\n", t);
+ if (!temp_sensor_exists)
+ return false;
+ t = temp_sensor;
+ return true;
}
-#endif
-// End of Interrupt Service Routines
-
-void setVI (double v, double i)
+void setVI_A (double v, double i)
{
MotorA.set_V_limit (v); // Sets max motor voltage
MotorA.set_I_limit (i); // Sets max motor current
+}
+
+void setVI_B (double v, double i)
+{
MotorB.set_V_limit (v);
MotorB.set_I_limit (i);
}
+void setVI_both (double v, double i)
+{
+ setVI_A (v, i);
+ setVI_B (v, i);
+}
+
/**
* void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
@@ -217,11 +240,11 @@
MotorA.high_side_off ();
if (MotorB.tickleon)
MotorB.high_side_off ();
- if (AtoD_Semaphore > 20) {
+ if (AtoD_Semaphore > 10) {
pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
- AtoD_Semaphore = 20;
+ AtoD_Semaphore = 10;
}
- while (AtoD_Semaphore > 0) {
+ while (AtoD_Semaphore > 0) { // semaphore gets incremented in timer interrupt handler, t=50us
AtoD_Semaphore--;
// Code here to sequence through reading voltmeter, demand pot, ammeters
switch (i) { //
@@ -254,36 +277,6 @@
}
}
-/** double Read_Servo1_In ()
-* driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
-* ISR also filters signal
-* This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
-*/
-double Read_Servo1_In ()
-{
- const double xjoin = 0.5,
- yjoin = 0.35,
- slope_a = yjoin / xjoin,
- slope_b = (1.0 - yjoin)/(1.0 - xjoin);
-// centre = 0.35, // For pot, first (1/3)ish in braking area, position = 1/3 drift, > (1/3)ish drive
-// halfish = 0.5; // RC stick in the centre value
- // Bottom half of RC stick movement maps to lowest (1/3)ish pot movement
- // Higher half of RC stick movement maps to highest (2/3)ish pot movement
- double t;
- double demand = RC_chan_1.normalised();
- // apply demand = 1.0 - demand here to swap stick move polarity
-// return pow (demand, SERVOIN_PWR_BENDER);
- if (demand < 0.0) demand = 0.0;
- if (demand > 1.0) demand = 1.0;
- if (demand < xjoin) {
- demand *= slope_a;
- }
- else {
- t = yjoin + ((demand - xjoin) * slope_b);
- demand = t;
- }
- return demand;
-}
/** double Read_DriverPot ()
* driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
@@ -301,71 +294,26 @@
}
-void Update_Current_Scaler () { // ***NEW Sept 2019*** Called at 8Hz
-const double Vnom = 48.0,
- Vmin = Vnom / 3.0,
- Voff = Vnom / 4.0;
-
- double v = Read_BatteryVolts ();
- if (v > Vnom)
- v = Vnom;
- if (v < Voff)
- v = Voff;
- if (v > Vmin) { // In expected normal operating voltage range
- Current_Scaler_Sep_2019 = v / Vnom; // May need to revisit law
- }
- else { // In very low voltage region
- Current_Scaler_Sep_2019 = 0.5 * (v / Vnom);
- }
+double trim (const double min, const double max, double input) {
+ if (input < min) input = min;
+ if (input > max) input = max;
+ return input;
}
-void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb
+void brake_motors_both (double brake_effort) {
+ MotorA.brake (brake_effort);
+ MotorB.brake (brake_effort);
+}
+
+
+void mode_set_motors_both (int mode) // called from cli to set fw, re, rb, hb
{
MotorA.set_mode (mode);
MotorB.set_mode (mode);
- if (mode == MOTOR_REGENBRAKE) {
- if (val > 1.0)
- val = 1.0;
- if (val < 0.0)
- val = 0.0;
- val *= 0.9; // set upper limit, this is essential
- val = sqrt (val); // to linearise effect
- setVI (val, 1.0);
- }
}
-
-#ifdef USING_DC_MOTORS
-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 ();
- }
-}
-#endif
-
-void rcin_report () {
- double c1 = RC_chan_1.normalised();
- double c2 = RC_chan_2.normalised();
- uint32_t pc1 = RC_chan_1.pulsecount();
- uint32_t pc2 = RC_chan_2.pulsecount();
- pc.printf ("At rcin_report, Ch1=%.3f, Ch2=%.3f, cnt1 %d, cnt2 %d\r\n", c1, c2, pc1, pc2);
-// if (c1 < -0.0001)
- pc.printf ("\t1 period %d, pulsewidth %d\r\n", RC_chan_1.period(), RC_chan_1.pulsewidth());
-// if (c2 < -0.0001)
- pc.printf ("\t2 period %d, pulsewidth %d\r\n", RC_chan_2.period(), RC_chan_2.pulsewidth());
-}
-
-bool worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes
+bool is_it_worth_the_bother (double a, double b) { // Tests size of change. No point updating tiny demand changes
double c = a - b;
if (c < 0.0)
c = 0.0 - c;
@@ -374,9 +322,9 @@
return false;
}
-void hand_control_state_machine (int source) { // if hand control mode '3', get here @ approx 30Hz to read drivers control pot
- // if servo1 mode '4', reads input from servo1 instead
-enum { // states used in hand_control_state_machine()
+void hand_control_state_machine (int source) { // if hand control user_settings '3', get here @ approx 30Hz to read drivers control pot
+ // if servo1 user_settings '4', reads input from servo1 instead
+enum { // states used in RC_or_hand_control_state_machine()
HAND_CONT_IDLE,
HAND_CONT_BRAKE_WAIT,
HAND_CONT_BRAKE_POT,
@@ -392,7 +340,7 @@
static int dirbuf[5] = {100,100,100,100,100}; // Initialised with invalid direction such that 'change' is read from switch
static int dirbufptr = 0, direction_old = -1, direction_new = -1;
const int buflen = sizeof(dirbuf) / sizeof(int);
- const double Pot_Brake_Range = 0.35; //pow (0.5, SERVOIN_PWR_BENDER); //0.353553 for SERVOIN_PWR_BENDER = 1.5;
+ double User_Brake_Range; //
direction_old = direction_new;
@@ -410,10 +358,9 @@
hand_controller_state = HAND_CONT_BRAKE_WAIT; // validated change of direction switch
switch (source) {
- case 3: // driver pot is control input
+ case HAND: // driver pot is control input
pot_position = Read_DriverPot (); // Only place read in the loop, rteads normalised 0.0 to 1.0
- break;
- case 4: // servo 1 input is control input
+ User_Brake_Range = user_settings.user_brake_range();
break;
default:
pot_position = 0.0;
@@ -427,14 +374,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 (MOTOR_REGENBRAKE, brake_effort); // Direction change
+ brake_motors_both (brake_effort);
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
+ if (brake_effort < 0.95) { // 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 (MOTOR_REGENBRAKE, brake_effort); // Direction change or testing at power on
+ brake_motors_both (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
@@ -445,24 +392,23 @@
break;
case HAND_CONT_STATE_INTO_BRAKING:
- brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
+ brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range;
if (brake_effort < 0.0)
brake_effort = 0.5;
- mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort);
+ brake_motors_both (brake_effort);
old_pot_position = pot_position;
hand_controller_state = HAND_CONT_STATE_BRAKING;
pc.printf ("Brake\r\n");
break;
case HAND_CONT_STATE_BRAKING:
- if (pot_position > Pot_Brake_Range) // escape braking into drive
+ if (pot_position > User_Brake_Range) // escape braking into drive
hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
else {
- if (worth_the_bother(pot_position, old_pot_position)) {
+ if (is_it_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 (MOTOR_REGENBRAKE, brake_effort);
-// pc.printf ("Brake %.2f\r\n", brake_effort);
+ brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range;
+ brake_motors_both (brake_effort);
}
}
break;
@@ -470,20 +416,20 @@
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 (MOTOR_FORWARD, 0.0); // sets both motors
+ mode_set_motors_both (MOTOR_FORWARD); // sets both motors
else
- mode_set_both_motors (MOTOR_REVERSE, 0.0);
+ mode_set_motors_both (MOTOR_REVERSE);
hand_controller_state = HAND_CONT_STATE_DRIVING;
break;
case HAND_CONT_STATE_DRIVING:
- if (pot_position < Pot_Brake_Range) // escape braking into drive
+ if (pot_position < User_Brake_Range) // escape braking into drive
hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
else {
- if (worth_the_bother(pot_position, old_pot_position)) {
+ if (is_it_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 ???
+ drive_effort = (pot_position - User_Brake_Range) / (1.0 - User_Brake_Range);
+ setVI_both (drive_effort, drive_effort); // Wind volts and amps up and down together ???
pc.printf ("Drive %.2f\r\n", drive_effort);
}
}
@@ -495,68 +441,81 @@
} // endof switch (hand_controller_state) {
}
-int main()
+void set_RCIN_offsets () {
+ RC_chan_1.set_offset (user_settings.rd(RCI1_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK));
+ RC_chan_2.set_offset (user_settings.rd(RCI2_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK));
+ RC_chan_1.set_chanmode (user_settings.rd(RCIN1), user_settings.rd(RCIN1REVERSE)) ;
+ RC_chan_2.set_chanmode (user_settings.rd(RCIN2), user_settings.rd(RCIN2REVERSE)) ;
+}
+
+
+int main() // Programme entry point
{
- int eighth_sec_count = 0;
+ uint32_t eighth_sec_count = 0;
// Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
- tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
+ tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator - 50 us
loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
-#ifdef TEMP_SENSOR_ENABLE
- temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960);
- Temperature_pin.fall (&temp_sensor_isr);
- Temperature_pin.mode (PullUp);
- temperature_timer.start ();
-#endif
+
// Done setting up system interrupt timers
- 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
+// pc.baud (9600); // COM port to pc
+ pc.baud (user_settings.baud()); // COM port to pc
- 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
+// pc.printf ("Baud rate %d\r\n", user_settings.baud());
- 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
+ MotorA.set_direction (user_settings.rd(MOTADIR)); // user_settingss all setup in class eeprom_settings {} user_settings ; constructor
+ MotorB.set_direction (user_settings.rd(MOTBDIR));
+ MotorA.poles (user_settings.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise
+ MotorB.poles (user_settings.rd(MOTBPOLES)); // Only two calls are here
+// MotorA.set_mode (MOTOR_REGENBRAKE); // Done in motor constructor
+// MotorB.set_mode (MOTOR_REGENBRAKE);
+// setVI_both (0.9, 0.5); // Power up with moderate regen braking applied
+ set_RCIN_offsets ();
+
+ class RC_stick_info RCstick1, RCstick2;
// 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
+// T3 = 0;// T4 = 0;// T5 = 0; now input from fw/re on remote control box
T6 = 0;
+ pc.printf ("\r\n\nSTM3_ESC Starting Ver %s, Command Source setting = %d\r\n", get_version(), user_settings.rd(COMM_SRC));
+ pc.printf ("Designed by Jon Freeman B. Eng. Hons - 2017-2020. e jon@jons-workshop.com\r\n");
+ if (user_settings.do_we_have_i2c (0xa0))
+ pc.printf ("Got EEPROM, i2c error count = %d\r\n", user_settings.errs());
+ else
+ pc.printf ("No eeprom found\r\n");
+ pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true - Ready to Roll !" : "false");
-#ifdef TEMP_SENSOR_ENABLE
- if ((last_temperature_count > 160) && (last_temperature_count < 2400)) // in range -40 to +100 degree C
+//bool eeprom_settings::do_we_have_i2c (uint32_t x)
+ pc.printf ("LM75 temp sensor ");
+ if (user_settings.do_we_have_i2c (LM75_I2C_ADDR)) {
+ pc.printf ("reports temperature %7.3f\r\n", (float)temp_sensor );
temp_sensor_exists = true;
-#endif
-#ifdef USING_DC_MOTORS
- dc_motor_kicker_timer.start ();
-#endif
- int old_hand_controller_direction = T5;
- if (mode.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401
+ }
+ else
+ pc.printf ("Not Fitted\r\n");
+
+ uint32_t old_hand_controller_direction = T5;
+ if (user_settings.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401
pc.printf ("Pot control\r\n");
if (T5)
- mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors
+ mode_set_motors_both (MOTOR_FORWARD); // sets both motors
else
- mode_set_both_motors (MOTOR_REVERSE, 0.0);
+ mode_set_motors_both (MOTOR_REVERSE);
}
-
- 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 () ;
WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup
-
+ pcc.flush (); // Flush serial rx buffers
+ tsc.flush ();
+// pc.printf ("sizeof int is %d\r\n", sizeof(int)); // sizeof int is 4
+ double Current_Scaler; // New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom.
+ // See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I
+ // at lower voltages. This is simply because current takes longer to build in motor inductance when voltage
+ // is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning
+ // similar current flows for shorter times at higher voltages.
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
@@ -566,62 +525,57 @@
} // 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
- 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
- break;
- case COM2: // COM2 - Primarily for testing, bypassed by command line interpreter
+ // double trim (const double min, const double max, double input) {
+ Current_Scaler = trim (0.1, 1.0, Read_BatteryVolts() / user_settings.Vnom());
+ MotorA.I_scale (Current_Scaler); // Reduces motor current limits when voltage below nominal.
+ MotorB.I_scale (Current_Scaler); // This goes some way towards preventing engine stalls perhaps
+
+ 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
+
+ RC_chan_1.read(RCstick1); // Get latest info from Radio Control inputs
+ RC_chan_2.read(RCstick2);
+
+//#define SERVO_OUT_TEST
+#ifdef SERVO_OUT_TEST
+ if (RCstick1.active) Servo1 = RCstick1.deflection;
+ if (RCstick2.active) Servo2 = RCstick2.deflection;
+#endif
+
+ switch (user_settings.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
+// break;
+ case COM2: // COM2 - Nothing done here, all serial instructions handled in command line interpreter
break;
case HAND: // Put all hand controller input stuff here
- hand_control_state_machine (3);
+ hand_control_state_machine (HAND);
break; // endof hand controller stuff
case RC_IN1: // RC_chan_1
- hand_control_state_machine (4);
+ RC_chan_1.energise (RCstick1, MotorA) ; // RC chan 1 drives both motors
+ RC_chan_1.energise (RCstick1, MotorB) ;
break;
case RC_IN2: // RC_chan_2
+ RC_chan_2.energise (RCstick2, MotorA) ; // RC chan 2 drives both motors
+ RC_chan_2.energise (RCstick2, MotorB) ;
+ break;
+ case RC_IN_BOTH: // Robot Mode
+ RC_chan_1.energise (RCstick1, MotorA) ; // Two RC chans drive two motors independently
+ RC_chan_2.energise (RCstick2, MotorB) ;
break;
default:
if (ESC_Error.read(FAULT_UNRECOGNISED_STATE)) {
- pc.printf ("Unrecognised state %d\r\n", mode.rd(COMM_SRC)); // set error flag instead here
+ pc.printf ("Unrecognised state %d\r\n", user_settings.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 ();
-#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
+ } // endof switch (user_settings_bytes[COMM_SRC]) {
-#ifdef USING_DC_MOTORS
- 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);
- }
-#endif
-#define SERVO_OUT_TEST
-#ifdef SERVO_OUT_TEST
+//#define SERVO_OUT_TEST
+//#ifdef SERVO_OUT_TEST
// static double servo_angle = 0.0; // For testing servo outs
// servo out test here December 2018
-
- // Tests for pulse width and repetition rates being believable signal from radio control
- if (RC_chan_1.validate_rx())
- Servo1 = RC_chan_1.normalised();
- if (RC_chan_2.validate_rx())
- Servo2 = RC_chan_2.normalised();
-// RC_chan_2.validate_rx();
-
-
/*
servo_angle += 0.01;
if (servo_angle > TWOPI)
@@ -630,31 +584,23 @@
Servo2 = ((cos(servo_angle)+1.0) / 2.0);
*/
// Yep! Both servo outs work lovely December 2018
-#endif
+//#endif
if (flag_8Hz) { // do slower stuff
flag_8Hz = false;
- LED = !LED; // Toggle LED on board, should be seen to fast flash
+ LED = !LED; // Toggle green LED on board, should be seen to fast flash
if (WatchDogEnable) {
WatchDog--;
if (WatchDog < 1) { // Deal with WatchDog timer timeout here
WatchDog = 0;
- setVI (0.0, 0.0); // set motor volts and amps to zero
-// 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
+ setVI_both (0.0, 0.0); // set motor volts and amps to zero
+ pc.printf ("TIMEOUT %c\r\n", user_settings.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this
} // End of dealing with WatchDog timer timeout
}
- Update_Current_Scaler ();
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;
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);
- }*/
}
} // End of if(flag_8Hz)
} // End of main programme loop