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
Diff: main.cpp
- Revision:
- 11:bfb73f083009
- Parent:
- 10:e40d8724268a
- Child:
- 12:d1d21a2941ef
--- a/main.cpp Tue Jan 15 09:03:57 2019 +0000
+++ b/main.cpp Sat Jan 19 11:45:01 2019 +0000
@@ -1,12 +1,14 @@
// Cloned from 'DualBLS2018_06' on 23 November 2018
#include "mbed.h"
//#include "users/mbed_official/code/mbed-dev/file/707f6e361f3e/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F401xE/device/stm32f401xe.h"
+#include "DualBLS.h"
+
#include "stm32f401xe.h"
-#include "DualBLS.h"
#include "BufferedSerial.h"
#include "FastPWM.h"
#include "Servo.h"
#include "brushless_motor.h"
+#include "Radio_Control_In.h"
/*
Brushless_STM3 board
@@ -50,10 +52,10 @@
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 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 eeprom_flag; // gets set according to 24LC674 being found or not
bool mode_good_flag = false;
char mode_bytes[36];
@@ -69,6 +71,9 @@
Timer temperature_timer;
Timer dc_motor_kicker_timer;
Timeout motors_restore;
+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
@@ -109,104 +114,17 @@
fast_sys_timer++; // Just a handy measure of elapsed time for anything to use
}
-
-/**class RControl_In
- Checks for __-__ duration 800-2200us
- Checks repetition rate in range 5-25ms
-*/
-class RControl_In
+void temp_sensor_isr () // got rising edge from LMT01. ALMOST CERTAIN this misses some
{
- // Read servo style pwm input
- Timer t;
- int32_t pulse_width_us, period_us, pulse_count;
-public:
- RControl_In () { // Default Constructor
- pulse_width_us = period_us = pulse_count = 0;
- rx_active = false;
- com2.printf ("Setting up Radio_Control_In\r\n");
- } ;
- bool validate_rx () ;
- void rise () ; // InterruptIn ISRs redirected to these
- void fall () ;
- uint32_t pulsecount () ;
- uint32_t pulsewidth () ;
- uint32_t period () ;
- double normalised (); // Returns 0.0 <= p <= 1.0 or something else when rc not active
- bool rx_active;
-} ;
-
-
-uint32_t RControl_In::pulsewidth ()
-{
- return pulse_width_us;
-}
-
-uint32_t RControl_In::pulsecount ()
-{
- return pulse_count;
-}
-
-uint32_t RControl_In::period ()
-{
- return period_us;
+ 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 RControl_In::validate_rx ()
-{ // Tests for pulse width and repetition rates being believable
- if ((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200))
- {
- rx_active = false;
- pc.printf ("RC per=%d, pw=%d\r\n", period_us, pulse_width_us);
- }
- else
- rx_active = true;
- return rx_active;
-}
-
-double RControl_In::normalised () {
- if (!validate_rx())
- return 0.0; // ** WHAT TO RETURN WHEN RC NOT ACTIVE ? **
- double norm = (double) (pulse_width_us - 1000); // left with -200 to + 1200 allowing for some margin
- norm /= 1000.0;
- if (norm > 1.0)
- norm = 1.0;
- if (norm < 0.0)
- norm = 0.0;
- return norm;
-}
-
-void RControl_In::rise () // These may not work as use of PortB as port may bugger InterruptIn use ** THIS IS SO **
-{ // December 2018 - ** FIXED ** by using PC_14 and 15 instead
-// t.stop ();
- period_us = t.read_us ();
- t.reset ();
- t.start ();
-}
-void RControl_In::fall ()
-{
- pulse_width_us = t.read_us ();
- pulse_count++;
-}
-// end of RControl_In class
-
- RControl_In RC_chan_1, RC_chan_2; // Declare two radio control input channels
-
-// Radio Control Input Interrupt Handlers
-void RC_chan_1rise () {
- RC_chan_1.rise ();
-}
-void RC_chan_1fall () {
- RC_chan_1.fall ();
-}
-void RC_chan_2rise () {
- RC_chan_2.rise ();
-}
-void RC_chan_2fall () {
- RC_chan_2.fall ();
-}
-// End of Radio Control Input Interrupt Handlers
-
-//Servo * Servos[2]; // Is possible to create pointers to Servo but no need to.
+// End of Interrupt Service Routines
//uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
/*
@@ -232,11 +150,7 @@
0, BRA,BRA,BRA,BRA,BRA,BRA,BRA, // Regenerative Braking
KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
} ;
-InterruptIn * AHarr[] = {
- &MAH1,
- &MAH2,
- &MAH3
-} ;
+
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
@@ -244,15 +158,9 @@
0, BRB,BRB,BRB,BRB,BRB,BRB,BRB, // Regenerative Braking
KEEP_L_MASK_B, KEEP_H_MASK_B
} ;
-InterruptIn * BHarr[] = {
- &MBH1,
- &MBH2,
- &MBH3
-} ;
-
-brushless_motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr);
-brushless_motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, BHarr);
+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);
void report_motor_types () // not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise
@@ -260,43 +168,6 @@
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++;
- if (t > 18) // Yes proved some interrupts get missed, this fixes temperature reading
- temp_sensor_count++;
-// T2 = !T2; // scope hanger
-}
-
-
-void MAH_isr ()
-{
- uint32_t x = 0;
- MotorA.high_side_off ();
-// T3 = !T3;
- if (MAH1 != 0) x |= 1;
- if (MAH2 != 0) x |= 2;
- if (MAH3 != 0) x |= 4;
- MotorA.Hindex[0] = x; // New in [0], old in [1]
- MotorA.Hall_change ();
-}
-
-void MBH_isr ()
-{
- uint32_t x = 0;
- MotorB.high_side_off ();
- if (MBH1 != 0) x |= 1;
- if (MBH2 != 0) x |= 2;
- if (MBH3 != 0) x |= 4;
- MotorB.Hindex[0] = x;
- MotorB.Hall_change ();
-}
-
-
-// End of Interrupt Service Routines
-
void setVI (double v, double i)
{
MotorA.set_V_limit (v); // Sets max motor voltage
@@ -304,37 +175,19 @@
MotorB.set_V_limit (v);
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 read_RPM (uint32_t * dest)
-{
- dest[0] = MotorA.RPM;
- dest[1] = MotorB.RPM;
-}
-
-void read_PPS (uint32_t * dest)
-{
- dest[0] = MotorA.PPS;
- dest[1] = MotorB.PPS;
-}
-
-void read_last_VI (double * d) // only for test from cli
-{
- d[0] = MotorA.last_V;
- d[1] = MotorA.last_I;
- d[2] = MotorB.last_V;
- d[3] = MotorB.last_I;
-}
-
/**
void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
@@ -342,7 +195,7 @@
*/
void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr
{
- static uint32_t i = 0, tab_ptr = 0;
+ static uint32_t i = 0;
// if (MotorA.dc_motor) {
// MotorA.low_side_on ();
// }
@@ -374,12 +227,10 @@
driverpot_reading >>= 1;
break;
case 2:
- MotorA.current_samples[tab_ptr] = Motor_A_Current.read_u16 (); //
+ MotorA.sniff_current (); // Initiate ADC reading into averaging table
break;
case 3:
- MotorB.current_samples[tab_ptr++] = Motor_B_Current.read_u16 (); //
- if (tab_ptr >= CURRENT_SAMPLES_AVERAGED) // Current reading will be lumpy and spikey, so put through moving average filter
- tab_ptr = 0;
+ MotorB.sniff_current ();
break;
}
i++; // prepare to read the next input in response to the next interrupt
@@ -444,13 +295,6 @@
return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct !
}
-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_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb
{
MotorA.set_mode (mode);
@@ -511,28 +355,6 @@
}
}
-//int ttime = 3; // resettable via cli 'stt', used to determine suitable force low on period for driving dc motor
-
-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);
-}
-
void rcin_report () {
double c1 = RC_chan_1.normalised();
double c2 = RC_chan_2.normalised();
@@ -545,7 +367,7 @@
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) {
+bool 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;
@@ -604,17 +426,17 @@
case HAND_CONT_IDLE: // Here for first few passes until validated direction switch reading
break;
- case HAND_CONT_BRAKE_WAIT:
+ 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
hand_controller_state = HAND_CONT_BRAKE_POT;
break;
- case HAND_CONT_BRAKE_POT:
- if (brake_effort < 0.9) {
- brake_effort += 0.05; // ramp braking up to max over about one sec
- mode_set_both_motors (REGENBRAKE, brake_effort); // Direction change
+ 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
pc.printf ("Brake effort %.2f\r\n", brake_effort);
}
else { // once braking up to quite hard
@@ -647,7 +469,7 @@
}
break;
- case HAND_CONT_STATE_INTO_DRIVING:
+ 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
@@ -680,47 +502,9 @@
int eighth_sec_count = 0;
double servo_angle = 0.0; // For testing servo outs
- MotA = 0; // Output all 0s to Motor drive ports A and B
- MotB = 0;
-// MotPtr[0] = &MotorA; // Pointers to motor class objects
-// MotPtr[1] = &MotorB;
Temperature_pin.fall (&temp_sensor_isr);
Temperature_pin.mode (PullUp);
-#ifdef RC1IN
- RC_1_in.rise (& RC_chan_1rise) ;
- RC_1_in.fall (& RC_chan_1fall) ;
- RC_1_in.mode (PullDown);
-#endif
-#ifdef RC2IN
- RC_2_in.rise (& RC_chan_2rise) ;
- RC_2_in.fall (& RC_chan_2fall) ;
- RC_2_in.mode (PullDown);
-#endif
-// Servo1_i.mode (PullUp); // These never worked, December 2018 re-trying using PC_14 and 15
-// Servo2_i.mode (PullUp);
-
- MAH1.rise (& MAH_isr); // Set up interrupt vectors
- MAH1.fall (& MAH_isr);
- MAH2.rise (& MAH_isr);
- MAH2.fall (& MAH_isr);
- MAH3.rise (& MAH_isr);
- MAH3.fall (& MAH_isr);
-
- MBH1.rise (& MBH_isr);
- MBH1.fall (& MBH_isr);
- MBH2.rise (& MBH_isr);
- MBH2.fall (& MBH_isr);
- MBH3.rise (& MBH_isr);
- MBH3.fall (& MBH_isr);
-
- MAH1.mode (PullUp);
- MAH2.mode (PullUp);
- MAH3.mode (PullUp);
- MBH1.mode (PullUp);
- MBH2.mode (PullUp);
- MBH3.mode (PullUp);
-
// Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
tick_vread.attach_us (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US); // Start periodic interrupt generator
loop_timer.attach_us (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US); // Start periodic interrupt generator
@@ -728,8 +512,6 @@
// Done setting up system interrupt timers
temperature_timer.start ();
-// const int TXTBUFSIZ = 36;
-// char buff[TXTBUFSIZ];
pc.baud (9600);
com3.baud (1200);
com2.baud (19200);
@@ -788,45 +570,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
- // However, setup code for Input by default,
- // This can be used to monitor Servo output also !
-
-// December 2018 ** NEED TO PROVE SERVO OUT WORKS ** YES, DONE.
- Servo Servo1 (PB_8) ;
-// 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.
- */ /*
- switch (mode_byte) { // executed once only upon startup
- case POT:
- while (1) { // forever loop
- call common_stuff ();
- ...
- }
- break;
- case COM2:
- while (1) { // forever loop
- call common_stuff ();
- ...
- }
- break;
- }
- */
// pc.printf ("Ready to go!, wheel gear in position %d\r\n", WHEELGEAR);
dc_motor_kicker_timer.start ();
int old_hand_controller_direction = T5;
@@ -839,18 +584,13 @@
}
pc.printf ("About to start!, mode_bytes[COMM_SRC]= %d\r\n", mode_bytes[COMM_SRC]);
-// const double pwr = 1.5;SERVOIN_PWR_BENDER
-// for (double i = 0.0; i < 1.05; i+= 0.1)
-// pc.printf ("%f ^ %f = %f\r\n", i, SERVOIN_PWR_BENDER, pow(i, SERVOIN_PWR_BENDER));
-
-// pc.printf ("PortA=%lx\r\n", PortA);
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
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
RC_chan_1.validate_rx();
@@ -859,20 +599,20 @@
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
+ case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter
break;
- case 2: // COM2 - Primarily for testing, bypassed by command line interpreter
+ case COM2: // COM2 - Primarily for testing, bypassed by command line interpreter
break;
- case 3: // Put all hand controller input stuff here
+ case HAND: // Put all hand controller input stuff here
hand_control_state_machine (3);
break; // endof hand controller stuff
- case 4: // Servo1
+ case RC_IN1: // RC_chan_1
hand_control_state_machine (4);
break;
- case 5: // Servo2
+ case RC_IN2: // RC_chan_2
break;
default:
- pc.printf ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]);
+ pc.printf ("Unrecognised state %d\r\n", mode_bytes[COMM_SRC]); // set error flag instead here
break;
} // endof switch (mode_bytes[COMM_SRC]) {
@@ -918,8 +658,10 @@
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
// servo out test here December 2018
servo_angle += 0.01;
if (servo_angle > TWOPI)
@@ -927,7 +669,8 @@
Servo1 = ((sin(servo_angle)+1.0) / 2.0);
Servo2 = ((cos(servo_angle)+1.0) / 2.0);
// Yep! Both servo outs work lovely December 2018
-
+#endif
} // End of if(flag_8Hz)
} // End of main programme loop
}
+