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
 }
+