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
Revision 11:bfb73f083009, committed 2019-01-19
- Comitter:
- JonFreeman
- Date:
- Sat Jan 19 11:45:01 2019 +0000
- Parent:
- 10:e40d8724268a
- Child:
- 12:d1d21a2941ef
- Commit message:
- Tidied class parameter passing, serial problem fixed (was hardware)
Changed in this revision
--- a/DualBLS.h Tue Jan 15 09:03:57 2019 +0000
+++ b/DualBLS.h Sat Jan 19 11:45:01 2019 +0000
@@ -1,3 +1,8 @@
+#include "mbed.h"
+
+#ifndef MBED_DUALBLS_H
+#define MBED_DUALBLS_H
+
const int HANDBRAKE = 0,
FORWARD = 8,
REVERSE = 16,
@@ -20,6 +25,8 @@
const double PI = 4.0 * atan(1.0),
TWOPI = 8.0 * atan(1.0);
+enum {COM_SOURCES, COM1, COM2, HAND, RC_IN1, RC_IN2,THEEND} ;
+
//enum {MOTADIR, MOTBDIR, GANG, SVO1, SVO2, COMM_SRC, ID, WHEELDIA, MOTPIN, WHEELGEAR} ; // Identical in TS and DualBLS
enum {MOTADIR, MOTBDIR, GANG, SVO1, SVO2, COMM_SRC, ID, WHEELDIA, MOTPIN, WHEELGEAR, BOGHUNWAT, FUT1, FUT2, FUT3, FUT4, FUT5} ; // Identical in TS and DualBLS
struct optpar {
@@ -51,3 +58,5 @@
} ;
//const double SERVOIN_PWR_BENDER = 1.5; // Used to change servo_in stick at centre position to match pot approx 1/3 braking 2/3 driving
+#endif
+
--- a/F401RE.h Tue Jan 15 09:03:57 2019 +0000 +++ b/F401RE.h Sat Jan 19 11:45:01 2019 +0000 @@ -4,11 +4,9 @@ // Problem - Appears to conflict with serial port used for comms with controller // Earlier efforts to use 'Servo' ports as 'you choose' between I/O failed as pins not capable of use as 'InterruptIn' -// Experiment disabling RC inputs to see if clearing serial conflict is possible +// CORRECTION Comms problem with Touch Screen was insufficient pull-up on STM3_ESC opto. Change R12 from 1k to 470R -//#define RC1IN -//#define RC2IN - +// Experiment disabling RC inputs to see if clearing serial conflict is possible // Port A -> MotorA, Port B -> MotorB @@ -67,8 +65,8 @@ 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); +//PortOut MotA (PortA, PORT_A_MASK); // Activate output ports to motor drivers +//PortOut MotB (PortB, PORT_B_MASK); // Pin 1 VBAT NET +3V3 @@ -76,13 +74,6 @@ 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 -#ifdef RC1IN -InterruptIn RC_1_in (PC_14); //New Dec 2018 tryiing to find pins to use for servo in -#endif -#ifdef RC2IN -InterruptIn RC_2_in (PC_15); //Yes, PC_14 and PC_15 do work -#endif - // Pin 3 PC14-OSC32_IN NET O32I Xtal chucked off these pins, now needed for RC inputs // Pin 4 PC15-OSC32_OUT NET O32O // Pin 5 PH0-OSC_IN NET PH1 @@ -90,8 +81,10 @@ // Pin 7 NRST NET NRST AnalogIn Ain_DriverPot (PC_0); // Pin 8 Spare Analogue in, net SAIN fitted with external pull-down AnalogIn Ain_SystemVolts (PC_1); // Pin 9 -AnalogIn Motor_A_Current (PC_2); // Pin 10 -AnalogIn Motor_B_Current (PC_3); // Pin 11 +#define MOT_A_I_ADC PC_2 +#define MOT_B_I_ADC PC_3 +//AnalogIn Motor_A_Current (PC_2); // Pin 10 +//AnalogIn Motor_B_Current (PC_3); // Pin 11 // Pin 12 VSSA/VREF- NET GND // Pin 13 VDDA/VREF+ NET +3V3 // Pin 14 Port_A AUL @@ -105,8 +98,10 @@ DigitalOut LED (PA_5); // Pin 21 // Pin 22 Port_A AVL // Pin 23 Port_A AVH -InterruptIn MBH2 (PC_4); // Pin 24 -InterruptIn MBH3 (PC_5); // Pin 25 +//InterruptIn MBH2 (PC_4); // Pin 24 +//InterruptIn MBH3 (PC_5); // Pin 25 +#define _MBH2 PC_4 +#define _MBH3 PC_5 // Pin 26 Port_B BUL // Pin 27 Port_B BVL // Pin 28 Port_B BWL @@ -119,9 +114,11 @@ DigitalOut T4 (PB_14); // Pin 35 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, PWM_PRESECALER_DEFAULT), // Pin 39 pwm3/3 - A_MAX_I_PWM (PC_9, PWM_PRESECALER_DEFAULT); // pin 40, prescaler value pwm3/4 +BufferedSerial com2 (PC_6, PC_7); // Pins 37, 38 tx, rx to Touch Screen Controller +#define APWMV PC_8 +#define APWMI PC_9 +//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 @@ -150,13 +147,19 @@ //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 (PC_10); // Pin 51 +//InterruptIn MAH2 (PC_11); // Pin 52 +//InterruptIn MAH3 (PC_12); // Pin 53 +#define _MAH1 PC_10 +#define _MAH2 PC_11 +#define _MAH3 PC_12 +//InterruptIn MBH1 (PD_2); // Pin 54 +#define _MBH1 PD_2 DigitalOut T6 (PB_3); // Pin 55 -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 +#define BPWMV PB_4 +#define BPWMI PB_5 +//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 @@ -175,4 +178,9 @@ // Pin 63 VSS // Pin 64 VDD // SYSTEM CONSTANTS +// December 2018 ** NEED TO PROVE SERVO OUT WORKS ** YES, DONE. + Servo Servo1 (PB_8) ; +// Servos[0] = & Servo1; + Servo Servo2 (PB_9) ; +// Servos[1] = & Servo2;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Radio_Control_In.cpp Sat Jan 19 11:45:01 2019 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "Radio_Control_In.h"
+/**class RControl_In
+ Jon Freeman
+ Jan 2019
+
+ Checks for __-__ duration 800-2200us
+ Checks repetition rate in range 5-25ms
+*/
+extern BufferedSerial pc;
+
+// RControl_In::RControl_In () { // Default Constructor
+// pulse_width_us = period_us = pulse_count = 0;
+// lost_chan_return_value = 0.0;
+// } ;
+// RControl_In::RControl_In (PinName inp) : pulse_in(inp) { // Default Constructor
+// pulse_width_us = period_us = pulse_count = 0;
+// lost_chan_return_value = 0.0;
+// } ;
+/**
+*/
+void RControl_In::set_lost_chan_return_value (double d) {
+ lost_chan_return_value = d;
+}
+
+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;
+}
+
+bool RControl_In::validate_rx ()
+{ // Tests for pulse width and repetition rates being believable
+ return !((period_us < 5000) || (period_us > 25000) || (pulse_width_us < 800) || (pulse_width_us > 2200));
+}
+
+double RControl_In::normalised () {
+ if (!validate_rx()) {
+ pc.printf ("Invalid RCin\r\n");
+ return lost_chan_return_value; // ** WHAT TO RETURN WHEN RC NOT ACTIVE ? ** This is now user settable
+ } // Defaults to returning 0.0, user should call set_lost_chan_value (value) to alter
+ 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::RadC_rise () // December 2018 - Could not make Servo port bidirectional, fix by using PC_14 and 15 as inputs
+{
+ period_us = t.read_us ();
+ t.reset ();
+ t.start ();
+}
+
+void RControl_In::RadC_fall ()
+{
+ pulse_width_us = t.read_us ();
+ pulse_count++;
+}
+// end of RControl_In class
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Radio_Control_In.h Sat Jan 19 11:45:01 2019 +0000
@@ -0,0 +1,37 @@
+#include "mbed.h"
+#ifndef MBED_JONS_RADIO_CONTROL_IN_H
+#define MBED_JONS_RADIO_CONTROL_IN_H
+
+/**class RControl_In
+ Jon Freeman
+ Jan 2019
+
+ Checks for __-__ duration 800-2200us
+ Checks repetition rate in range 5-25ms
+*/
+class RControl_In // Class to Read servo style pwm input _____-_____
+{
+ InterruptIn pulse_in;
+ Timer t;
+ int32_t pulse_width_us, period_us, pulse_count;
+ double lost_chan_return_value;
+ void RadC_rise ();
+ void RadC_fall ();
+public:
+// RControl_In () ; // Default Constructor
+ RControl_In (PinName inp) : pulse_in(inp) { // Default Constructor
+ pulse_in.mode (PullDown);
+ pulse_in.rise(callback(this, &RControl_In::RadC_rise)); // Attach handler to the rising interruptIn edge
+ pulse_in.fall(callback(this, &RControl_In::RadC_fall)); // Attach handler to the falling interruptIn edge
+ pulse_width_us = period_us = pulse_count = 0;
+ lost_chan_return_value = 0.0;
+ } ;
+ bool validate_rx () ; // Informs whether signal being rx'd
+ void set_lost_chan_return_value (double) ; // set what 'normalised' returns when no signal
+ uint32_t pulsecount () ; // will count up at frame rate when radio control all working well
+ uint32_t pulsewidth () ;
+ uint32_t period () ;
+ double normalised (); // Returns 0.0 <= p <= 1.0 or something else when rc not active
+} ;
+
+#endif
--- a/brushless_motor.cpp Tue Jan 15 09:03:57 2019 +0000
+++ b/brushless_motor.cpp Sat Jan 19 11:45:01 2019 +0000
@@ -7,44 +7,77 @@
#include "FastPWM.h"
#include "Servo.h"
#include "brushless_motor.h"
-brushless_motor::brushless_motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor
+
+
+brushless_motor::brushless_motor (PinName iadc, PinName pwv, PinName pwi,
+ const uint16_t * lutptr, PinName px, PinName py, PinName pz, PortName pn, int port_bit_mask)
+ : Motor_I(iadc), maxV(pwv,PWM_PRESECALER_DEFAULT), maxI(pwi,PWM_PRESECALER_DEFAULT), H1(px), H2(py), H3(pz), OP(pn, port_bit_mask) // Constructor
{
// Constructor
- maxV = _maxV_;
- maxI = _maxI_;
+ OP = 0;
+ H1.mode (PullUp);
+ H2.mode (PullUp);
+ H3.mode (PullUp);
+ H1.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
+ H1.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
+ H2.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
+ H2.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
+ H3.rise (callback(this, &brushless_motor::Hall_change)); // Attach handler to the rising interruptIn edge
+ H3.fall (callback(this, &brushless_motor::Hall_change)); // Attach handler to the falling interruptIn edge
Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
latest_pulses_per_sec = 0;
for (int i = 0; i < MAIN_LOOP_ITERATION_Hz; i++)
edge_count_table[i] = 0;
-// if (lutptr != A_tabl && lutptr != B_tabl)
-// pc.printf ("Fatal in 'motor' constructor, Invalid lut address\r\n");
Hall_tab_ptr = 0;
- Motor_Port = P;
-// pc.printf ("In motor constructor, Motor port = %lx\r\n", P);
- maxV->period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
- maxI->period_ticks (MAX_PWM_TICKS + 1);
- maxV->pulsewidth_ticks (MAX_PWM_TICKS / 20);
- maxI->pulsewidth_ticks (MAX_PWM_TICKS / 30);
+ maxV.period_ticks (MAX_PWM_TICKS + 1); // around 18 kHz
+ maxI.period_ticks (MAX_PWM_TICKS + 1);
+ maxV.pulsewidth_ticks (MAX_PWM_TICKS / 20);
+ maxI.pulsewidth_ticks (MAX_PWM_TICKS / 30);
visible_mode = REGENBRAKE;
inner_mode = REGENBRAKE;
lut = lutptr;
- Hindex[0] = Hindex[1] = read_Halls ();
+ Hall_index[0] = Hall_index[1] = read_Halls ();
ppstmp = 0;
+ cs_ptr = 0;
tickleon = 0;
direction = 0;
angle_cnt = 0; // Incremented or decremented on each Hall event according to actual measured direction of travel
encoder_error_cnt = 0; // Incremented when Hall transition not recognised as either direction
- Hall1 = Hall[0];
- Hall2 = Hall[1];
- Hall3 = Hall[2];
PPS = 0;
RPM = 0;
last_V = last_I = 0.0;
- int x = read_Halls ();
- if (x == 7)
- dc_motor = true;
+ dc_motor = (Hall_index[0] == 7) ? true : false ;
+}
+
+void brushless_motor::sniff_current () {
+ current_samples[cs_ptr++] = Motor_I.read_u16 (); //
+ if (cs_ptr >= CURRENT_SAMPLES_AVERAGED)
+ cs_ptr = 0;
+}
+
+void brushless_motor::Hall_change () {
+ int32_t delta_theta;
+ 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
+ } ;
+
+ Hall_index[0] = read_Halls ();
+ delta_theta = delta_theta_lut[(Hall_index[1] << 3) | Hall_index[0]];
+
+ if (delta_theta == 0)
+ encoder_error_cnt++;
else
- dc_motor = false;
+ angle_cnt += delta_theta;
+ OP = lut[inner_mode | Hall_index[0]]; // changed mode to inner_mode 27/04/18
+ Hall_total++;
+ Hall_index[1] = Hall_index[0];
}
bool brushless_motor::motor_is_brushless ()
@@ -71,24 +104,27 @@
int brushless_motor::read_Halls ()
{
int x = 0;
- if (*Hall1 != 0) x |= 1;
- if (*Hall2 != 0) x |= 2;
- if (*Hall3 != 0) x |= 4;
+ if (H1 != 0) x |= 1;
+ if (H2 != 0) x |= 2;
+ if (H3 != 0) x |= 4;
return x;
}
void brushless_motor::high_side_off ()
{
- uint16_t p = *Motor_Port;
+// uint16_t p = *Motor_Port;
+ uint16_t p = OP;
p &= lut[32]; // KEEP_L_MASK_A or B
- *Motor_Port = p;
+// *Motor_Port = p;
+ OP = p;
}
void brushless_motor::low_side_on ()
{
// uint16_t p = *Motor_Port;
// p &= lut[31]; // KEEP_L_MASK_A or B
- *Motor_Port = lut[31];
+// *Motor_Port = lut[31];
+ OP = lut[31];
}
void brushless_motor::current_calc ()
@@ -113,7 +149,7 @@
{
if (v < 1) v = 1;
if (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
- maxV->pulsewidth_ticks (v);
+ maxV.pulsewidth_ticks (v);
}
void brushless_motor::set_V_limit (double p) // Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
@@ -125,7 +161,7 @@
last_V = p; // for read by diagnostics
p *= 0.95; // need limit, ffi see MCP1630 data
p = 1.0 - p; // because pwm is wrong way up
- maxV->pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
+ maxV.pulsewidth_ticks ((int)(p * MAX_PWM_TICKS)); // PWM output to MCP1630 inverted motor pwm as MCP1630 inverts
}
void brushless_motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
@@ -141,7 +177,7 @@
a = MAX_PWM_TICKS;
if (a < 0)
a = 0;
- maxI->pulsewidth_ticks (a); // PWM
+ maxI.pulsewidth_ticks (a); // PWM
}
uint32_t brushless_motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
@@ -194,30 +230,10 @@
return true;
}
-void brushless_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
- } ;
- int32_t delta_theta = delta_theta_lut[(Hindex[1] << 3) | Hindex[0]];
- if (delta_theta == 0)
- encoder_error_cnt++;
- else
- angle_cnt += delta_theta;
- *Motor_Port = lut[inner_mode | Hindex[0]]; // changed mode to inner_mode 27/04/18
- Hall_total++;
- Hindex[1] = Hindex[0];
-}
void brushless_motor::motor_set ()
{
- Hindex[0] = read_Halls ();
- *Motor_Port = lut[inner_mode | Hindex[0]];
+ Hall_index[0] = read_Halls ();
+// *Motor_Port = lut[inner_mode | Hindex[0]];
+ OP = lut[inner_mode | Hall_index[0]];
}
--- a/brushless_motor.h Tue Jan 15 09:03:57 2019 +0000
+++ b/brushless_motor.h Sat Jan 19 11:45:01 2019 +0000
@@ -6,68 +6,50 @@
#define MBED_BRUSHLESSMOTOR_H
#include "mbed.h"
+#include "DualBLS.h"
class brushless_motor
{
+ int32_t angle_cnt;
+ uint32_t Hall_index[2], encoder_error_cnt;
uint32_t Hall_total, visible_mode, inner_mode, edge_count_table[MAIN_LOOP_ITERATION_Hz]; // to contain one seconds worth
uint32_t latest_pulses_per_sec, Hall_tab_ptr, direction, ppstmp;
bool moving_flag;
const uint16_t * lut;
uint16_t ttabl[34];
- FastPWM * maxV, * maxI;
- PortOut * Motor_Port;
- InterruptIn * Hall1, * Hall2, * Hall3;
+ FastPWM maxV;
+ FastPWM maxI;
+ InterruptIn H1;
+ InterruptIn H2;
+ InterruptIn H3;
+ PortOut OP;
+ AnalogIn Motor_I;
+ void Hall_change () ;
+ int read_Halls () ; // Returns 3 bits of latest Hall sensor outputs
public:
bool dc_motor;
struct currents {
uint32_t max, min, ave;
} I;
- int32_t angle_cnt;
uint32_t current_samples[CURRENT_SAMPLES_AVERAGED]; // Circular buffer where latest current readings get stored
- uint32_t Hindex[2], tickleon, encoder_error_cnt;
- uint32_t RPM, PPS;
+ uint32_t cs_ptr;
+ uint32_t RPM, PPS, tickleon;
double last_V, last_I;
- brushless_motor () {} ; // Default constructor
-/**
- brushless_motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ;
-
- PortOut * is &MotA where MotA has been declared as below where PORT_A_MASK identifies which 6 bits of 16 bit port are used to energise motor
- PortOut MotA (PortA, PORT_A_MASK); // Activate output ports to motor drivers
-
- FastPWM * are e.g. &A_MAX_V_PWM, &A_MAX_I_PWM
-
- const uint16_t * is e.g. A_tabl defining which port bits set to which level
-const uint16_t A_tabl[] = { // Origial table
- 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
- 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 ** is e.g. AHarr pointer to array of addresses of InterruptIns connected to Hall sensors
-InterruptIn * AHarr[] = {
- &MAH1,
- &MAH2,
- &MAH3
-} ;
-
-*/
- brushless_motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ;
+// brushless_motor () {} ; // can not use this with exotic elements PortOut, FastPWM etc
+ brushless_motor (PinName iadc, PinName pwv, PinName pwi, const uint16_t *, PinName h1, PinName h2, PinName h3, PortName, int) ; // Constructor
void set_V_limit (double) ; // Sets max motor voltage
void set_I_limit (double) ; // Sets max motor current
- void Hall_change () ; // Called in response to edge on Hall input pin
void motor_set () ; // Energise Port with data determined by Hall sensors
void direction_set (int) ; // sets 'direction' with bit pattern to eor with FORWARD or REVERSE in set_mode
bool set_mode (int); // sets mode to HANDBRAKE, FORWARD, REVERSE or REGENBRAKE
bool is_moving () ; // Returns true if one or more Hall transitions within last 31.25 milli secs
void current_calc () ; // Updates 3 uint32_t I.min, I.ave, I.max
uint32_t pulses_per_sec () ; // call this once per main loop pass to keep count = edges per sec
- 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);
+ void sniff_current () ;
} ; //MotorA, MotorB, or even Motor[2];
#endif
--- a/cli_BLS_nortos.cpp Tue Jan 15 09:03:57 2019 +0000
+++ b/cli_BLS_nortos.cpp Sat Jan 19 11:45:01 2019 +0000
@@ -1,6 +1,9 @@
// DualBLS2018_06
#include "mbed.h"
#include "BufferedSerial.h"
+#include "FastPWM.h"
+#include "DualBLS.h"
+#include "brushless_motor.h"
#include <cctype>
#include "DualBLS.h"
@@ -11,6 +14,8 @@
extern bool WatchDogEnable;
extern char mode_bytes[];
+extern brushless_motor MotorA, MotorB;
+
const int BROADCAST = '\r';
const int MAX_PARAMS = 20;
struct parameters {
@@ -30,10 +35,11 @@
extern void setVI (double v, double i) ;
extern void setV (double v) ;
extern void setI (double i) ;
-extern void read_last_VI (double * val) ; // only for test from cli
+//extern void last_VI (double * val) ; // only for test from cli
//BufferedSerial * com;
extern double Read_DriverPot ();
+extern double Read_BatteryVolts ();
void pot_cmd (struct parameters & a)
{
pc.printf ("Driver's pot %.3f\r\n", Read_DriverPot ());
@@ -69,12 +75,6 @@
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)
{
@@ -84,24 +84,17 @@
}
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
{
- if (a.respond) {
- double r[4];
- read_supply_vi (r); // get MotorA.I.ave, MotorB.I.ave, Battery volts
- a.com->printf ("rdi%.0f %.0f %.1f\r", r[0], r[1], r[2]); // Format good to be unpicked by cli in touch screen controller
- }
+ if (a.respond)
+ a.com->printf ("rdi%.0f %.0f %.1f\r", MotorA.I.ave, MotorB.I.ave, Read_BatteryVolts ()); // Format good to be unpicked by cli in touch screen controller
}
void rvi_cmd (struct parameters & a) // read last normalised values sent to pwms
{
- if (a.respond) {
- double r[6];
- read_last_VI (r);
- a.com->printf ("rvi%.2f %.2f %.2f %.2f\r", r[0], r[1], r[2], r[3]);
- }
+ if (a.respond)
+ a.com->printf ("rvi%.2f %.2f %.2f %.2f\r", MotorA.last_V, MotorA.last_I, MotorB.last_V, MotorB.last_I);
}
void fw_cmd (struct parameters & a) // Forward command
@@ -233,24 +226,17 @@
}
}
-extern void read_RPM (uint32_t * dest) ;
void rpm_cmd (struct parameters & a) // to report e.g. RPM 1000 1000 ; speed for both motors
{
- if (a.respond) {
- uint32_t dest[3];
- read_RPM (dest); // gets rpm for each motor
- a.com->printf ("rpm%d %d\r", dest[0], dest[1]);
- }
+ if (a.respond)
+ a.com->printf ("rpm%d %d\r", MotorA.RPM, MotorB.RPM);
}
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", mode_bytes[ID], (double)(dest[0] + dest[1]) * rpm2mph / 2.0);
- }
+ if (a.respond)
+ a.com->printf ("mph%c %.3f\r", mode_bytes[ID], (double)(MotorA.RPM + MotorB.RPM) * rpm2mph / 2.0);
}
void menucmd (struct parameters & a);
@@ -343,7 +329,6 @@
{"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},
@@ -405,7 +390,7 @@
*/
//void command_line_interpreter (void const *argument)
void cli_core (struct parameters & a) {
- const int MAX_CMD_LEN = 120;
+ const int MAX_CMD_LEN = 180;
int ch, IAm = I_Am();
char * pEnd;//, * cmd_line_ptr;
while (a.com->readable()) {
@@ -414,8 +399,10 @@
a.com->printf ("Error!! Stupidly long cmd line\r\n");
a.cl_index = 0;
}
- if(ch != '\r') // was this the 'Enter' key?
- a.cmd_line[a.cl_index++] = ch; // added char to command being assembled
+ if(ch != '\r') { // was this the 'Enter' key?
+ if (ch != '\n') // Ignore line feeds
+ a.cmd_line[a.cl_index++] = ch; // added char to command being assembled
+ }
else { // key was CR, may or may not be command to lookup
a.target_unit = BROADCAST; // Set to BROADCAST default once found command line '\r'
a.cmd_line_ptr = a.cmd_line;
--- a/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
}
+
--- a/mbed.bld Tue Jan 15 09:03:57 2019 +0000 +++ b/mbed.bld Sat Jan 19 11:45:01 2019 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/7130f322cb7e \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file