Jon Freeman
/
Brushless_STM3_Ctrl_2018_11
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed BufferedSerial Servo FastPWM
Diff: main.cpp
- Revision:
- 10:e40d8724268a
- Parent:
- 9:ac2412df01be
- Child:
- 11:bfb73f083009
--- a/main.cpp Sat Nov 10 17:08:21 2018 +0000
+++ b/main.cpp Tue Jan 15 09:03:57 2019 +0000
@@ -1,3 +1,4 @@
+// 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 "stm32f401xe.h"
@@ -5,10 +6,13 @@
#include "BufferedSerial.h"
#include "FastPWM.h"
#include "Servo.h"
+#include "brushless_motor.h"
/*
-New 29th May 2018 - YET TO CODE FOR - Fwd/Rev line from possible remote hand control box has signal routed to T5
- Also new LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable
+Brushless_STM3 board
+
+New 29th May 2018 **
+ LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable
*/
@@ -35,7 +39,7 @@
#include "F401RE.h" // See here for warnings about Servo InterruptIn not working
#endif
#if defined (TARGET_NUCLEO_F446ZE) // CPU in 144 pin LQFP
-#include "F446ZE.h"
+#include "F446ZE.h" // A thought for future version
#endif
/* Global variable declarations */
volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
@@ -106,47 +110,74 @@
}
+/**class RControl_In
+ Checks for __-__ duration 800-2200us
+ Checks repetition rate in range 5-25ms
+*/
class RControl_In
{
// Read servo style pwm input
Timer t;
- int32_t clock_old, clock_new;
- int32_t pulse_width_us, period_us;
+ int32_t pulse_width_us, period_us, pulse_count;
public:
- RControl_In () {
- pulse_width_us = period_us = 0;
- com2.printf ("Setting up Radio_Congtrol_In\r\n");
+ 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 () ;
+ 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;
}
bool RControl_In::validate_rx ()
-{
- if ((clock() - clock_new) > 4)
+{ // 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;
}
-void RControl_In::rise () // These may not work as use of PortB as port may bugger InterruptIn use
-{
- t.stop ();
+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 ();
@@ -154,14 +185,28 @@
void RControl_In::fall ()
{
pulse_width_us = t.read_us ();
- clock_old = clock_new;
- clock_new = clock();
- if ((clock_new - clock_old) < 4)
- rx_active = true;
+ 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];
+//Servo * Servos[2]; // Is possible to create pointers to Servo but no need to.
//uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0;
/*
@@ -182,8 +227,8 @@
*/
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, 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
KEEP_L_MASK_A, KEEP_H_MASK_A // [32 and 33]
} ;
@@ -194,8 +239,8 @@
} ;
const uint16_t B_tabl[] = {
0, 0, 0, 0, 0, 0, 0, 0, // Handbrake
- 0, BWV,BVU,BWU,BUW,BUV,BVW,BUW, // Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0, // JP, FR, SG, PWM = 1 0 1 1 Forward1
- 0, BVW,BUV,BUW,BWU,BVU,BWV,BWU, // Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0, // JP, FR, SG, PWM = 1 1 0 1 Reverse1
+ 0, 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
KEEP_L_MASK_B, KEEP_H_MASK_B
} ;
@@ -205,256 +250,9 @@
&MBH3
} ;
-class motor
-{
- 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;
- FastPWM * maxV, * maxI;
- PortOut * Motor_Port;
- InterruptIn * Hall1, * Hall2, * Hall3;
-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;
- double last_V, last_I;
- motor () {} ; // Default constructor
- motor (PortOut * , FastPWM * , FastPWM * , const uint16_t *, InterruptIn **) ;
- 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);
-} ; //MotorA, MotorB, or even Motor[2];
-motor MotorA (&MotA, &A_MAX_V_PWM, &A_MAX_I_PWM, A_tabl, AHarr);
-motor MotorB (&MotB, &B_MAX_V_PWM, &B_MAX_I_PWM, B_tabl, BHarr);
-
-//motor * MotPtr[8]; // Array of pointers to some number of motor objects
-
-motor::motor (PortOut * P , FastPWM * _maxV_ , FastPWM * _maxI_ , const uint16_t * lutptr, InterruptIn ** Hall) // Constructor
-{
- // Constructor
- maxV = _maxV_;
- maxI = _maxI_;
- Hall_total = 0; // mode can be only 0, 8, 16 or 24, lut row select for Handbrake, Forward, Reverse, or Regen Braking
- 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);
- visible_mode = REGENBRAKE;
- inner_mode = REGENBRAKE;
- lut = lutptr;
- Hindex[0] = Hindex[1] = read_Halls ();
- ppstmp = 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;
- else
- dc_motor = false;
-}
-
-bool motor::motor_is_brushless ()
-{
- /* int x = read_Halls ();
- if (x < 1 || x > 6)
- return false;
- return true;
- */
- return !dc_motor;
-}
-
-/**
-void motor::direction_set (int dir) {
-Used to set direction according to mode data from eeprom
-*/
-void motor::direction_set (int dir)
-{
- if (dir != 0)
- dir = FORWARD | REVERSE; // bits used in eor
- direction = dir;
-}
-
-int motor::read_Halls ()
-{
- int x = 0;
- if (*Hall1 != 0) x |= 1;
- if (*Hall2 != 0) x |= 2;
- if (*Hall3 != 0) x |= 4;
- return x;
-}
-
-void motor::high_side_off ()
-{
- uint16_t p = *Motor_Port;
- p &= lut[32]; // KEEP_L_MASK_A or B
- *Motor_Port = p;
-}
-
-void motor::low_side_on ()
-{
-// uint16_t p = *Motor_Port;
-// p &= lut[31]; // KEEP_L_MASK_A or B
- *Motor_Port = lut[31];
-}
-
-void motor::current_calc ()
-{
- I.min = 0x0fffffff; // samples are 16 bit
- I.max = 0;
- I.ave = 0;
- uint16_t sample;
- for (int i = 0; i < CURRENT_SAMPLES_AVERAGED; i++) {
- sample = current_samples[i];
- I.ave += sample;
- if (I.min > sample)
- I.min = sample;
- if (I.max < sample)
- I.max = sample;
- }
- I.ave /= CURRENT_SAMPLES_AVERAGED;
-}
-
-
-void motor::raw_V_pwm (int v)
-{
- if (v < 1) v = 1;
- if (v > MAX_PWM_TICKS) v = MAX_PWM_TICKS;
- maxV->pulsewidth_ticks (v);
-}
-
-void motor::set_V_limit (double p) // Sets max motor voltage. Use set_V_limit (last_V) to restore previous setting
-{
- if (p < 0.0)
- p = 0.0;
- if (p > 1.0)
- p = 1.0;
- 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
-}
-
-void motor::set_I_limit (double p) // Sets max motor current. pwm integrated to dc ref voltage level
-{
- int a;
- if (p < 0.0)
- p = 0.0;
- if (p > 1.0)
- p = 1.0;
- last_I = p;
- a = (int)(p * MAX_PWM_TICKS);
- if (a > MAX_PWM_TICKS)
- a = MAX_PWM_TICKS;
- if (a < 0)
- a = 0;
- maxI->pulsewidth_ticks (a); // PWM
-}
-
-uint32_t motor::pulses_per_sec () // call this once per 'MAIN_LOOP_REPEAT_TIME_US= 31250' main loop pass to keep count = edges per sec
-{
- // Can also test for motor running or not here
- if (dc_motor)
- return 0;
- if (ppstmp == Hall_total) {
-// if (dc_motor || ppstmp == Hall_total) {
- moving_flag = false; // Zero Hall transitions since previous call - motor not moving
- tickleon = TICKLE_TIMES;
- } else {
- moving_flag = true;
- ppstmp = Hall_total;
- }
- latest_pulses_per_sec = ppstmp - edge_count_table[Hall_tab_ptr];
- edge_count_table[Hall_tab_ptr] = ppstmp;
- Hall_tab_ptr++;
- if (Hall_tab_ptr >= MAIN_LOOP_ITERATION_Hz)
- Hall_tab_ptr = 0;
- PPS = latest_pulses_per_sec;
- RPM = (latest_pulses_per_sec * 60) / 24;
- return latest_pulses_per_sec;
-}
-
-bool motor::is_moving ()
-{
- return moving_flag;
-}
-
-/**
-bool motor::set_mode (int m)
-Use to set motor to one mode of HANDBRAKE, FORWARD, REVERSE, REGENBRAKE.
-If this causes change of mode, also sets V and I to zero.
-*/
-bool motor::set_mode (int m)
-{
- if ((m != HANDBRAKE) && (m != FORWARD) && (m != REVERSE) && (m !=REGENBRAKE)) {
- pc.printf ("Error in set_mode, invalid mode %d\r\n", m);
- return false;
- }
- if (visible_mode != m) { // Mode change, kill volts and amps to be safe
- set_V_limit (0.0);
- set_I_limit (0.0);
- visible_mode = m;
- }
- if (m == FORWARD || m == REVERSE)
- m ^= direction;
- inner_mode = m; // idea is to use inner_mode only in lut addressing, keep 'visible_mode' true regardless of setup data in eeprom
- return true;
-}
-
-void 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];
-}
+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);
void report_motor_types () // not very good test, shows 'Brushless' if Hall inputs read 1 to 6, 'DC' otherwise
@@ -499,12 +297,6 @@
// End of Interrupt Service Routines
-void motor::motor_set ()
-{
- Hindex[0] = read_Halls ();
- *Motor_Port = lut[inner_mode | Hindex[0]];
-}
-
void setVI (double v, double i)
{
MotorA.set_V_limit (v); // Sets max motor voltage
@@ -606,9 +398,40 @@
}
}
+/** 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
-* ISR also filters signal
+* ISR also filters signal by returning average of most recent two readings
* This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
*/
double Read_DriverPot ()
@@ -689,16 +512,6 @@
}
//int ttime = 3; // resettable via cli 'stt', used to determine suitable force low on period for driving dc motor
-bool worth_the_bother (double a, double b) {
- double c = a - b;
- if (c < 0.0)
- c = 0.0 - c;
- if (c > 0.02) {
-// pc.printf ("%.3f\r\n", c);
- return true;
- }
- return false;
-}
void prscfuck (int v) {
/*
@@ -720,54 +533,110 @@
pc.printf ("Attempt setting prescaler %d returned %d\r\n", v, w);
}
-enum { HAND_CONT_STATE_BEGIN,
- HAND_CONT_STATE_POWER_CYCLE_TO_EXIT,
- HAND_CONT_STATE_INTO_BRAKING,
- HAND_CONT_STATE_INTO_DRIVING,
- HAND_CONT_STATE_BRAKING,
- HAND_CONT_STATE_DRIVING
- } ;
+void 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());
+}
-void hand_control_state_machine () {
- static int new_hand_controller_state = HAND_CONT_STATE_BEGIN;
-// static int old_hand_controller_state = HAND_CONT_STATE_BEGIN;
- static int old_hand_controller_direction = T5, t = 0; // Nov 2018 confirms Rob and Quentin obs, direction read at powerup
+bool worth_the_bother (double a, double b) {
+ double c = a - b;
+ if (c < 0.0)
+ c = 0.0 - c;
+ if (c > 0.02)
+ return true;
+ 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()
+ HAND_CONT_IDLE,
+ HAND_CONT_BRAKE_WAIT,
+ HAND_CONT_BRAKE_POT,
+ HAND_CONT_STATE_INTO_BRAKING,
+ HAND_CONT_STATE_BRAKING,
+ HAND_CONT_STATE_INTO_DRIVING,
+ HAND_CONT_STATE_DRIVING
+ } ;
+
+ static int hand_controller_state = HAND_CONT_IDLE;
+// static int old_hand_controller_direction = T5; // Nov 2018 reworked thanks to feedback from Rob and Quentin
static double brake_effort, drive_effort, pot_position, old_pot_position = 0.0;
- if (T5 != old_hand_controller_direction) { // 1 Forward, 0 Reverse
- pc.printf ("Direction change! Power off then on again to resume\r\n");
- mode_set_both_motors (REGENBRAKE, 1.0);
-// old_hand_controller_state = new_hand_controller_state;
- new_hand_controller_state = HAND_CONT_STATE_POWER_CYCLE_TO_EXIT;
+ 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;
+
+ direction_old = direction_new;
+
+// Test for change in direction switch setting.
+// If whole buffer NEWLY filled with all Fwd or all Rev, state = brake_wait
+ int diracc = 0;
+ if (dirbufptr >= buflen)
+ dirbufptr = 0;
+ dirbuf[dirbufptr++] = T5; // Read direction switch into circular debounce buffer
+ for (int i = 0; i < buflen; i++)
+ diracc += dirbuf[i]; // will = 0 or buflen if direction switch stable
+ if (diracc == buflen || diracc == 0) // if was all 0 or all 1
+ direction_new = diracc / buflen;
+ if (direction_new != direction_old)
+ hand_controller_state = HAND_CONT_BRAKE_WAIT; // validated change of direction switch
+
+ switch (source) {
+ case 3: // 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
+ break;
+ default:
+ pot_position = 0.0;
+ break;
}
- pot_position = Read_DriverPot (); // gets to here on first pass before pot has been read
- switch (new_hand_controller_state) {
- case HAND_CONT_STATE_BEGIN:
- pot_position = Read_DriverPot ();
- if (t++ > 2 && pot_position < 0.02) {
- pc.printf ("In BEGIN, pot %.2f\r\n", pot_position);
- new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
+
+ switch (hand_controller_state) {
+ case HAND_CONT_IDLE: // Here for first few passes until validated direction switch reading
+ break;
+
+ case HAND_CONT_BRAKE_WAIT:
+ 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
+ pc.printf ("Brake effort %.2f\r\n", brake_effort);
+ }
+ else { // once braking up to quite hard
+ if (pot_position < 0.02) { // Driver has turned pot fully anticlock
+ hand_controller_state = HAND_CONT_STATE_BRAKING;
+ }
}
break;
- case HAND_CONT_STATE_POWER_CYCLE_TO_EXIT:
- break;
+
case HAND_CONT_STATE_INTO_BRAKING:
brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range;
+ if (brake_effort < 0.0)
+ brake_effort = 0.5;
mode_set_both_motors (REGENBRAKE, brake_effort);
old_pot_position = pot_position;
- new_hand_controller_state = HAND_CONT_STATE_BRAKING;
+ hand_controller_state = HAND_CONT_STATE_BRAKING;
pc.printf ("Brake\r\n");
break;
- case HAND_CONT_STATE_INTO_DRIVING:
- new_hand_controller_state = HAND_CONT_STATE_DRIVING;
- pc.printf ("Drive\r\n");
- if (T5)
- mode_set_both_motors (FORWARD, 0.0); // sets both motors
- else
- mode_set_both_motors (REVERSE, 0.0);
- break;
+
case HAND_CONT_STATE_BRAKING:
if (pot_position > Pot_Brake_Range) // escape braking into drive
- new_hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
+ hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
else {
if (worth_the_bother(pot_position, old_pot_position)) {
old_pot_position = pot_position;
@@ -777,9 +646,19 @@
}
}
break;
+
+ case HAND_CONT_STATE_INTO_DRIVING:
+ pc.printf ("Drive\r\n");
+ if (direction_new == 1)
+ mode_set_both_motors (FORWARD, 0.0); // sets both motors
+ else
+ mode_set_both_motors (REVERSE, 0.0);
+ hand_controller_state = HAND_CONT_STATE_DRIVING;
+ break;
+
case HAND_CONT_STATE_DRIVING:
if (pot_position < Pot_Brake_Range) // escape braking into drive
- new_hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
+ hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
else {
if (worth_the_bother(pot_position, old_pot_position)) {
old_pot_position = pot_position;
@@ -789,7 +668,9 @@
}
}
break;
+
default:
+ pc.printf ("Unhandled Hand Controller state %d\r\n", hand_controller_state);
break;
} // endof switch (hand_controller_state) {
}
@@ -797,6 +678,7 @@
int main()
{
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;
@@ -805,6 +687,18 @@
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);
@@ -826,8 +720,6 @@
MBH1.mode (PullUp);
MBH2.mode (PullUp);
MBH3.mode (PullUp);
- Servo1_i.mode (PullUp);
- Servo2_i.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
@@ -898,15 +790,17 @@
// prscfuck (PWM_PRESECALER_DEFAULT); // Test fucking with fastpwm prescaler
- Servos[0] = Servos[1] = NULL;
+// 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;
+// Servos[0] = & Servo1;
Servo Servo2 (PB_9) ;
- Servos[1] = & Servo2;
+// 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
@@ -945,6 +839,11 @@
}
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
@@ -952,7 +851,11 @@
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
}
- loop_flag = false; // Clear flag set by ticker interrupt handler
+ loop_flag = false; // Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms
+
+ RC_chan_1.validate_rx();
+ RC_chan_2.validate_rx();
+
switch (mode_bytes[COMM_SRC]) { // Look to selected source of driving command, act on commands from wherever
case 0: // Invalid
break;
@@ -961,9 +864,10 @@
case 2: // COM2 - Primarily for testing, bypassed by command line interpreter
break;
case 3: // Put all hand controller input stuff here
- hand_control_state_machine ();
+ hand_control_state_machine (3);
break; // endof hand controller stuff
case 4: // Servo1
+ hand_control_state_machine (4);
break;
case 5: // Servo2
break;
@@ -1015,6 +919,15 @@
}*/
// 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);
}
+
+ // servo out test here December 2018
+ servo_angle += 0.01;
+ if (servo_angle > TWOPI)
+ servo_angle -= TWOPI;
+ Servo1 = ((sin(servo_angle)+1.0) / 2.0);
+ Servo2 = ((cos(servo_angle)+1.0) / 2.0);
+ // Yep! Both servo outs work lovely December 2018
+
} // End of if(flag_8Hz)
} // End of main programme loop
}