main.cpp

00001 /*
00002     STM3_ESC    Electronic Speed Controller board, drives Two Brushless Motors, full Four Quadrant Control.
00003     Jon Freeman  B. Eng Hons
00004     2015 - 2020
00005 */
00006 #include "mbed.h"
00007 #include "STM3_ESC.h"
00008 #include "BufferedSerial.h"
00009 #include "FastPWM.h"
00010 #include "Servo.h"
00011 #include "brushless_motor.h"
00012 #include "Radio_Control_In.h"
00013 #include "LM75B.h"              //  New I2C temp sensor code March 2020 (to suit possible next board issue, harmless otherwise)
00014 //#ifdef  TARGET_NUCLEO_F401RE    //    Target is TARGET_NUCLEO_F401RE for all boards produced.
00015 //#endif
00016 /*
00017 Brushless_STM3_ESC board
00018 Apr 2020    *   RC tested on 'Idle Halt' branch line - all good - also tested Inverter Gen power sorce. All good.
00019 Dec 2019    *   Radio control inputs now fully implemented, requires fitting tiny 'RC_in_fix' board.
00020 Jan 2019    *   WatchDog implemented. Default is disabled, 'kd' command from TS controller enables and reloads
00021             *   Tidied brushless_motor class, parameter passing now done properly
00022             *   class   RControl_In created. Inputs now routed to new pins, can now use two chans both class   RControl_In and Servo out
00023                 (buggery board required for new inputs on June 2018 issue boards)
00024             *   Added version string
00025             *   Error handler written and included
00026             *   Realised Nanotec motors are 6 pole, all others are 8 pole. Modified 'mode' to include 'motor poles' in EEPROM data, now speed reading correct for all
00027             *   Reorganised EEPROM data - mode setting now much easier, less error prone
00028             *   Maximum speed now one EEPROM option, range 1.0 to 25.0 MPH in 0.1 MPH steps
00029                 
00030 New 29th May 2018 **
00031                 LMT01 temperature sensor routed to T1 - and rerouted to PC_13 as InterruptIn on T1 (ports A and B I think) not workable
00032         March 2019 temp sensor only included with TEMP_SENSOR_ENABLE defined. Temp reading not essential, LMT01 was not a good choice due to
00033         significant loading of interrupts, threatening integrity of Real Time System
00034         *** New sensor code for LM75B temp sensor added March 2020 ***
00035 */
00036 
00037 
00038 /*  STM32F401RE - compile using NUCLEO-F401RE
00039 //  PROJECT -   STM3_ESC Dual Brushless Motor Controller -   Jon Freeman     June 2018.
00040 
00041 AnalogIn to read each motor current
00042 
00043 ____________________________________________________________________________________
00044         CONTROL PHILOSOPHY
00045 This STM3_ESC Dual Brushless Motor drive board software should ensure sensible control when commands supplied are not sensible !
00046 
00047 That is, smooth transition between Drive, Coast and Brake to be implemented here.
00048 The remote controller must not be relied upon to deliver sensible command sequences.
00049 
00050 So much the better if the remote controller does issue sensible commands, but ...
00051 
00052 ____________________________________________________________________________________
00053 
00054 
00055 */
00056 
00057 //#if defined (TARGET_NUCLEO_L476RG)  //  CPU in 64 pin LQFP  ** NOT PROVED ** No good, pinmap different
00058 //#include    "F401RE.h"
00059 //#endif
00060 #if defined (TARGET_NUCLEO_F401RE)  //  CPU in 64 pin LQFP  -   This is what all produced boards have
00061 #include    "F401RE.h"
00062 #endif
00063 #if defined (TARGET_NUCLEO_F411RE)  //  CPU in 64 pin LQFP  -   Never tried, but probably would work as is
00064 #include    "F411RE.h"
00065 #endif
00066 #if defined (TARGET_NUCLEO_F446ZE)  //  CPU in 144 pin LQFP
00067 #include    "F446ZE.h"              //  A thought for future version
00068 #endif
00069 /*  Global variable declarations */
00070 
00071 //volatile    uint32_t    fast_sys_timer      = 0;    //  gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US
00072 uint32_t    WatchDog            = 0,    //  Set this up in main once pre-flight checks done. Allow extra few seconds at powerup
00073             volt_reading        = 0,    //  Global updated by interrupt driven read of Battery Volts
00074             driverpot_reading   = 0,    //  Global updated by interrupt driven read of Drivers Pot
00075             sys_timer           = 0,    //  gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US, 31250us at Sept 2019
00076             AtoD_Semaphore      = 0;
00077 
00078 bool        WatchDogEnable  = false;    //  Must recieve explicit instruction from pc or controller to enable
00079 bool        loop_flag   = false;        //  made true in ISR_loop_timer, picked up and made false again in main programme loop
00080 bool        flag_8Hz    = false;        //  As loop_flag but repeats 8 times per sec
00081 bool        temp_sensor_exists = false; //  March 2020 - Now used to indicate presence or not of LM75B i2c temp sensor
00082 
00083 /*
00084     *   Not used since LMT01 proved not a good choice. See LM75B code added March 2020
00085     *
00086 #ifdef  TEMP_SENSOR_ENABLE
00087 uint32_t    temp_sensor_count = 0,  //  incremented by every rising edge from LMT01
00088             last_temperature_count = 0;  //  global updated approx every 100ms after each LMT01 conversion completes
00089 Ticker  temperature_find_ticker;
00090 Timer   temperature_timer;
00091 #endif
00092 */
00093 /*  End of Global variable declarations */
00094 
00095 Ticker  tick_vread;     //  Device to cause periodic interrupts, used to time voltage readings etc - 50 us
00096 Ticker  loop_timer;     //  Device to cause periodic interrupts, used to sync iterations of main programme loop
00097 
00098 eeprom_settings             user_settings    (SDA_PIN, SCL_PIN)  ;   //  This MUST come before Motors setup
00099 RControl_In                 RC_chan_1   (PC_14);
00100 RControl_In                 RC_chan_2   (PC_15);   //  Instantiate two radio control input channels and specify which InterruptIn pin
00101 error_handling_Jan_2019     ESC_Error    ;         //  Provides array usable to store error codes.
00102 
00103 //PCT2075 temp_sensor( i2c );    //  or LM75B temp_sensor( p?, p? );  Added March 2020
00104 PCT2075 temp_sensor( SDA_PIN, SCL_PIN );    //  or LM75B temp_sensor( p?, p? );  Added March 2020
00105 
00106 
00107 /*
00108     5   1   3   2   6   4  Brushless Motor Hall SENSOR SEQUENCE
00109 
00110 1   1   1   1   0   0   0  ---___---___ Hall1
00111 2   0   0   1   1   1   0  __---___---_ Hall2
00112 4   1   0   0   0   1   1  -___---___-- Hall3
00113 
00114     UV  WV  WU  VU  VW  UW  OUTPUT SEQUENCE
00115 
00116 Dec 2019 Support for DC motors deleted.
00117 Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7.
00118 
00119 */
00120 const   uint16_t    A_tabl[] = {  //  Table of motor energisation bit patterns. Rows are Handbrake, Forward, Reverse, Regen brake. Cols relate to Hall sensor outputs
00121 //        AUHVL|AWL,  AWHUL|AVL,  AWHVL|AUH,  AVHWL|AUL,  AUHWL|AVH,  AVHUL|AWH
00122     0,  AUHVL|AWL,  AWHUL|AVL,  AWHVL|AUH,  AVHWL|AUL,  AUHWL|AVH,  AVHUL|AWH,    0,  //  Handbrake
00123 //      1->3        2->6        3->2        4->5        5->1        6->4
00124     0,  AWHVL,      AVHUL,      AWHUL,      AUHWL,      AUHVL,      AVHWL,  0,  //  Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,
00125 //      1->5        2->3        3->1        4->6        5->4        6->2
00126     0,  AVHWL,      AUHVL,      AUHWL,      AWHUL,      AVHUL,      AWHVL,  0,  //  Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,
00127     0,  BRA,        BRA,        BRA,        BRA,        BRA,        BRA,    BRA,   //  Regenerative Braking
00128     KEEP_L_MASK_A, KEEP_H_MASK_A   //  [32 and 33]
00129 }   ;
00130 //        AUHVL|AWL,  AWHUL|AVL,  AWHVL|AUH,  AVHWL|AUL,  AUHWL|AVH,  AVHUL|AWH
00131 const   uint16_t    B_tabl[] = {    //  Different tables for Motors A and B as different ports and different port bits used.
00132 //    0,    0,      0,      0,      0,      0,      0,    0,  //  Handbrake
00133     0,  BUHVL|BWL,  BWHUL|BVL,  BWHVL|BUH,  BVHWL|BUL,  BUHWL|BVH,  BVHUL|BWH,    0,  //  Handbrake
00134     0,  BWHVL,      BVHUL,      BWHUL,      BUHWL,      BUHVL,      BVHWL,    0,  //  Forward 0, WV1, VU1, WU1, UW1, UV1, VW1, 0,
00135     0,  BVHWL,      BUHVL,      BUHWL,      BWHUL,      BVHUL,      BWHVL,  0,  //  Reverse 0, VW1, UV1, UW1, WU1, VU1, WV1, 0,
00136     0,  BRB,        BRB,        BRB,        BRB,        BRB,        BRB,    BRB,   //  Regenerative Braking
00137     KEEP_L_MASK_B, KEEP_H_MASK_B
00138 }   ;
00139 
00140 brushless_motor   MotorA  (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK, ISHUNTA);
00141 brushless_motor   MotorB  (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB);    //  Two brushless motor instantiations
00142 
00143 extern  cli_2019    pcc, tsc;   //  command line interpreters from pc and touch screen
00144 
00145 static const int    LM75_I2C_ADDR = 0x90;   //  i2c temperature sensor
00146 
00147 //  Interrupt Service Routines
00148 /*
00149 #ifdef  TEMP_SENSOR_ENABLE
00150 void    ISR_temperature_find_ticker ()      //  every 960 us, i.e. slightly faster than once per milli sec
00151 {
00152     static  bool    readflag = false;
00153     int t = temperature_timer.read_ms   ();
00154     if  ((t == 5) && (!readflag))    {
00155         last_temperature_count = temp_sensor_count;
00156         temp_sensor_count = 0;
00157         readflag = true;
00158     }
00159     if  (t == 6)
00160         readflag = false;
00161 }
00162 
00163 void    temp_sensor_isr ()      //  got rising edge from LMT01. ALMOST CERTAIN this misses some
00164 {
00165     int t = temperature_timer.read_us   (); //  Must be being overrun by something, most likely culprit A-D reading ?
00166     temperature_timer.reset ();
00167     temp_sensor_count++;
00168     if  (t > 18)            //  Yes proved some interrupts get missed, this fixes temperature reading
00169         temp_sensor_count++;
00170 //    T2 = !T2;   //  scope hanger
00171 }
00172 #endif
00173 */
00174 
00175 /** void    ISR_loop_timer  ()
00176 *   This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above)
00177 *   This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop.
00178 *   Increments global 'sys_timer', usable anywhere as general measure of elapsed time
00179 */
00180 void    ISR_loop_timer  ()      //  This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US (32Hz)
00181 {
00182     loop_flag = true;   //  set flag to allow main programme loop to proceed
00183     sys_timer++;        //  Just a handy measure of elapsed time for anything to use
00184     if  ((sys_timer & 0x03) == 0)
00185         flag_8Hz    = true;
00186 }
00187 
00188 /** void    ISR_voltage_reader  ()
00189 *   This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds
00190 *
00191 *   AtoD_reader() called from convenient point in code to take readings outside of ISRs
00192 */
00193 void    ISR_voltage_reader  ()      //  This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US    = 50
00194 {
00195     AtoD_Semaphore++;
00196 //    fast_sys_timer++;        //  Just a handy measure of elapsed time for anything to use
00197 }
00198 
00199 //  End of Interrupt Service Routines
00200 
00201 const char *    get_version    ()  {
00202     return  "1.0.y2020.m05.d21\0";  //  Version string, readable using 'ver' serial command
00203 }
00204 
00205 /*bool    read_temperature    (float & t) {
00206 //    pc.printf   ("test param temp = %7.3f\r\n", t);
00207     if  (!temp_sensor_exists)
00208         return  false;
00209     t = temp_sensor;
00210     return  true;
00211 }*/
00212 
00213 void    setVI_A   (double v, double i)
00214 {
00215     MotorA.set_V_limit  (v);  //  Sets max motor voltage
00216     MotorA.set_I_limit  (i);  //  Sets max motor current
00217 }
00218 
00219 void    setVI_B   (double v, double i)
00220 {
00221     MotorB.set_V_limit  (v);
00222     MotorB.set_I_limit  (i);
00223 }
00224 
00225 void    setVI_both   (double v, double i)
00226 {
00227     setVI_A (v, i);
00228     setVI_B (v, i);
00229 }
00230 
00231 
00232 /**
00233 *   void    AtoD_reader ()  //  Call to here every VOLTAGE_READ_INTERVAL_US    = 50 once loop responds to flag set in isr
00234 *   Not part of ISR
00235 */
00236 void    AtoD_reader ()  //  Call to here every VOLTAGE_READ_INTERVAL_US    = 50 once loop responds to flag set in isr
00237 {
00238     static uint32_t i = 0;
00239     if  (MotorA.tickleon)
00240         MotorA.high_side_off    ();
00241     if  (MotorB.tickleon)
00242         MotorB.high_side_off    ();
00243     if  (AtoD_Semaphore > 10)   {
00244         pc.printf   ("WARNING - sema cnt %d\r\n", AtoD_Semaphore);
00245         AtoD_Semaphore = 10;
00246     }
00247     while   (AtoD_Semaphore > 0)    {   //  semaphore gets incremented in timer interrupt handler, t=50us
00248         AtoD_Semaphore--;
00249         //  Code here to sequence through reading voltmeter, demand pot, ammeters
00250         switch  (i) {   //
00251             case    0:
00252                 volt_reading += Ain_SystemVolts.read_u16    ();     //  Result = Result + New Reading
00253                 volt_reading >>= 1;                                 //  Result = Result / 2
00254                 break;                                              //  i.e. Very simple digital low pass filter
00255             case    1:
00256                 driverpot_reading += Ain_DriverPot.read_u16    ();
00257                 driverpot_reading >>= 1;
00258                 break;
00259             case    2:
00260                 MotorA.sniff_current    (); //  Initiate ADC current reading
00261                 break;
00262             case    3:
00263                 MotorB.sniff_current    ();
00264                 break;
00265         }
00266         i++;    //  prepare to read the next input in response to the next interrupt
00267         if  (i > 3)
00268             i = 0;
00269     }   //  end of while (AtoD_Semaphore > 0)    {
00270     if  (MotorA.tickleon)   {
00271         MotorA.tickleon--;
00272         MotorA.motor_set    (); //  Reactivate any high side switches turned off above
00273     }
00274     if  (MotorB.tickleon)   {
00275         MotorB.tickleon--;
00276         MotorB.motor_set    ();
00277     }
00278 }
00279 
00280 
00281 /** double  Read_DriverPot  ()
00282 *   driverpot_reading is a global 16 bit unsigned int updated in interrupt service routine
00283 *   ISR also filters signal by returning average of most recent two readings
00284 *   This function returns normalised double (range 0.0 to 1.0) representation of driver pot position
00285 */
00286 double  Read_DriverPot  ()
00287 {
00288     return ((double) driverpot_reading) / 65536.0;     //  Normalise 0.0 <= control pot <= 1.0
00289 }
00290 
00291 double  Read_BatteryVolts   ()
00292 {
00293     return  ((double) volt_reading) / 951.0;    //  divisor fiddled to make voltage reading correct !
00294 }
00295 
00296 
00297 double  trim    (const double min, const double max, double input)  {
00298     if  (input < min)   input = min;
00299     if  (input > max)   input = max;
00300     return  input;
00301 }
00302 
00303 void    brake_motors_both    (double brake_effort)  {
00304     MotorA.brake    (brake_effort);
00305     MotorB.brake    (brake_effort);
00306 }
00307 
00308 
00309 void    mode_set_motors_both   (int mode)      //  called from cli to set fw, re, rb, hb
00310 {
00311     MotorA.set_mode (mode);
00312     MotorB.set_mode (mode);
00313 }
00314 
00315 
00316 bool    is_it_worth_the_bother    (double a, double b)    {   //  Tests size of change. No point updating tiny demand changes
00317     double c = a - b;
00318     if  (c < 0.0)
00319         c = 0.0 - c;
00320     if  (c > 0.02)
00321         return  true;
00322     return  false;
00323 }
00324 
00325 void    hand_control_state_machine  (int source)  {     //  if hand control user_settings '3', get here @ approx 30Hz to read drivers control pot
00326                                                         //  if servo1 user_settings '4', reads input from servo1 instead
00327 enum    {   //  states used in RC_or_hand_control_state_machine()
00328         HAND_CONT_IDLE,
00329         HAND_CONT_BRAKE_WAIT,
00330         HAND_CONT_BRAKE_POT,
00331         HAND_CONT_STATE_INTO_BRAKING,
00332         HAND_CONT_STATE_BRAKING,
00333         HAND_CONT_STATE_INTO_DRIVING,
00334         HAND_CONT_STATE_DRIVING
00335         }  ;
00336 
00337     static  int hand_controller_state = HAND_CONT_IDLE;
00338 //    static  int old_hand_controller_direction = T5;              //  Nov 2018 reworked thanks to feedback from Rob and Quentin
00339     static  double  brake_effort, drive_effort, pot_position, old_pot_position = 0.0;
00340     static  int dirbuf[5] = {100,100,100,100,100};      //  Initialised with invalid direction such that 'change' is read from switch
00341     static  int dirbufptr = 0, direction_old = -1, direction_new = -1;
00342     const   int buflen = sizeof(dirbuf) / sizeof(int);
00343     double      User_Brake_Range;  //
00344 
00345    direction_old = direction_new;
00346 
00347 //      Test for change in direction switch setting.
00348 //      If whole buffer NEWLY filled with all Fwd or all Rev, state = brake_wait
00349     int diracc = 0;    
00350     if  (dirbufptr >= buflen)
00351         dirbufptr = 0;
00352     dirbuf[dirbufptr++] = T5;   //  Read direction switch into circular debounce buffer
00353     for (int i = 0; i < buflen; i++)
00354         diracc += dirbuf[i];    //  will = 0 or buflen if direction switch stable
00355     if  (diracc == buflen || diracc == 0)   //  if was all 0 or all 1
00356         direction_new = diracc / buflen;
00357     if  (direction_new != direction_old)
00358         hand_controller_state = HAND_CONT_BRAKE_WAIT;   //  validated change of direction switch
00359 
00360     switch  (source)    {
00361         case    HAND:  //  driver pot is control input
00362             pot_position = Read_DriverPot   ();     //  Only place read in the loop, rteads normalised 0.0 to 1.0
00363             User_Brake_Range = user_settings.user_brake_range();
00364             break;
00365         default:
00366             pot_position = 0.0;
00367             break;
00368     }
00369 
00370     switch  (hand_controller_state) {
00371         case    HAND_CONT_IDLE:         //  Here for first few passes until validated direction switch reading
00372             break;
00373 
00374         case    HAND_CONT_BRAKE_WAIT:   //  Only get here after direction input changed or newly validated at power on
00375             pc.printf   ("At HAND_CONT_BRAKE_WAIT\r\n");
00376             brake_effort = 0.05;    //  Apply braking not too fiercely
00377             brake_motors_both   (brake_effort);
00378             hand_controller_state = HAND_CONT_BRAKE_POT;
00379             break;
00380 
00381         case    HAND_CONT_BRAKE_POT:        //  Only get here after one pass through HAND_CONT_BRAKE_WAIT but
00382             if  (brake_effort < 0.95)    {   //   remain in this state until driver has turned pott fully anticlockwise
00383                 brake_effort += 0.05;   //  ramp braking up to max over about one sec after direction change or validation
00384                 brake_motors_both    (brake_effort);  //  Direction change or testing at power on
00385                 pc.printf   ("Brake effort %.2f\r\n", brake_effort);
00386             }
00387             else    {   //  once braking up to quite hard
00388                 if  (pot_position < 0.02)   {   //  Driver has turned pot fully anticlock
00389                     hand_controller_state = HAND_CONT_STATE_BRAKING;
00390                 }
00391             }
00392             break;
00393 
00394         case    HAND_CONT_STATE_INTO_BRAKING:
00395             brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range;
00396             if  (brake_effort < 0.0)
00397                 brake_effort = 0.5;
00398             brake_motors_both    (brake_effort);
00399             old_pot_position = pot_position;
00400             hand_controller_state = HAND_CONT_STATE_BRAKING;
00401             pc.printf   ("Brake\r\n");
00402             break;
00403 
00404         case    HAND_CONT_STATE_BRAKING:
00405             if  (pot_position > User_Brake_Range)    //  escape braking into drive
00406                 hand_controller_state = HAND_CONT_STATE_INTO_DRIVING;
00407             else    {
00408                 if  (is_it_worth_the_bother(pot_position, old_pot_position))  {
00409                     old_pot_position = pot_position;
00410                     brake_effort = (User_Brake_Range - pot_position) / User_Brake_Range;
00411                     brake_motors_both    (brake_effort);
00412                 }
00413             }
00414             break;
00415 
00416         case    HAND_CONT_STATE_INTO_DRIVING:   //  Only get here after HAND_CONT_STATE_BRAKING
00417             pc.printf   ("Drive\r\n");
00418             if  (direction_new == 1)
00419                 mode_set_motors_both   (MOTOR_FORWARD);  //  sets both motors
00420             else
00421                 mode_set_motors_both   (MOTOR_REVERSE);
00422             hand_controller_state = HAND_CONT_STATE_DRIVING;
00423             break;
00424 
00425         case    HAND_CONT_STATE_DRIVING:
00426             if  (pot_position < User_Brake_Range)    //  escape braking into drive
00427                 hand_controller_state = HAND_CONT_STATE_INTO_BRAKING;
00428             else    {
00429                 if  (is_it_worth_the_bother(pot_position, old_pot_position))  {
00430                     old_pot_position = pot_position;
00431                     drive_effort = (pot_position - User_Brake_Range) / (1.0 - User_Brake_Range);
00432                     setVI_both  (drive_effort, drive_effort);   //  Wind volts and amps up and down together ???
00433                     pc.printf   ("Drive %.2f\r\n", drive_effort);
00434                 }
00435             }
00436             break;
00437 
00438         default:
00439             pc.printf   ("Unhandled Hand Controller state %d\r\n", hand_controller_state);
00440             break;
00441     }       //  endof switch  (hand_controller_state) {
00442 }
00443 
00444 void    set_RCIN_offsets    ()  {
00445     RC_chan_1.set_offset    (user_settings.rd(RCI1_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK));
00446     RC_chan_2.set_offset    (user_settings.rd(RCI2_TRIM), user_settings.rd(RCIN_REGBRAKE_RANGE), user_settings.rd(RCIN_STICK_ATTACK));
00447     RC_chan_1.set_chanmode  (user_settings.rd(RCIN1), user_settings.rd(RCIN1REVERSE))  ;
00448     RC_chan_2.set_chanmode  (user_settings.rd(RCIN2), user_settings.rd(RCIN2REVERSE))  ;
00449 }
00450 
00451 void    rcins_report    ()  {
00452     pc.printf   ("RC1 pulsewidth %d, period %d, pulsecount %d\r\n", RC_chan_1.pulsewidth(), RC_chan_1.period(), RC_chan_1.pulsecount());
00453     pc.printf   ("RC2 pulsewidth %d, period %d, pulsecount %d\r\n", RC_chan_2.pulsewidth(), RC_chan_2.period(), RC_chan_2.pulsecount());
00454     pc.printf   ("\r\n");
00455 }
00456 
00457 int main()  //  Programme entry point
00458 {
00459     uint32_t    eighth_sec_count = 0;
00460     //  Setup system timers to cause periodic interrupts to synchronise and automate volt and current readings, loop repeat rate etc
00461     tick_vread.attach_us    (&ISR_voltage_reader, VOLTAGE_READ_INTERVAL_US);    //  Start periodic interrupt generator - 50 us
00462     loop_timer.attach_us    (&ISR_loop_timer, MAIN_LOOP_REPEAT_TIME_US);    //  Start periodic interrupt generator
00463 
00464     //  Done setting up system interrupt timers
00465 
00466     com3.baud   (1200);     //  Once had an idea to use this for IR comms, never tried
00467     com2.baud   (19200);    //  Opto isolated serial bus connecting 'n' STM3_ESC boards to 1 Brute Touch Screen controller
00468 //    pc.baud     (9600);     //  COM port to pc
00469     pc.baud     (user_settings.baud());     //  COM port to pc
00470 
00471 //    pc.printf   ("Baud rate %d\r\n", user_settings.baud());
00472 
00473     MotorA.set_direction    (user_settings.rd(MOTADIR));     //  user_settingss all setup in class   eeprom_settings {}  user_settings    ; constructor
00474     MotorB.set_direction    (user_settings.rd(MOTBDIR));
00475     MotorA.poles            (user_settings.rd(MOTAPOLES));   //  Returns true if poles 4, 6 or 8. Returns false otherwise
00476     MotorB.poles            (user_settings.rd(MOTBPOLES));   //  Only two calls are here
00477 //    MotorA.set_mode         (MOTOR_REGENBRAKE);   //  Done in motor constructor
00478 //    MotorB.set_mode         (MOTOR_REGENBRAKE);
00479 //    setVI_both  (0.9, 0.5);              //  Power up with moderate regen braking applied
00480     set_RCIN_offsets    ();
00481 
00482     class  RC_stick_info   RCstick1, RCstick2;
00483 
00484 //    T1 = 0;   Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor
00485     T2 = 0; //  T2, T3, T4 As yet unused pins
00486 //    T3 = 0;//    T4 = 0;//    T5 = 0; now input from fw/re on remote control box
00487     T6 = 0;
00488     pc.printf   ("\r\n\nSTM3_ESC Starting Ver %s, Command Source setting = %d\r\n", get_version(), user_settings.rd(COMM_SRC));
00489     pc.printf   ("Designed by Jon Freeman  B. Eng. Hons - 2017-2020. e jon@jons-workshop.com\r\n");
00490     if  (user_settings.do_we_have_i2c  (0xa0))
00491         pc.printf   ("Got EEPROM, i2c error count = %d\r\n", user_settings.errs());
00492     else
00493         pc.printf   ("No eeprom found\r\n");
00494     pc.printf   ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true - Ready to Roll !" : "false");
00495 
00496 //bool    eeprom_settings::do_we_have_i2c  (uint32_t x)
00497     pc.printf   ("LM75 temp sensor ");
00498     if  (user_settings.do_we_have_i2c  (LM75_I2C_ADDR))    {
00499         pc.printf   ("reports temperature %7.3f\r\n", (float)temp_sensor );
00500         temp_sensor_exists  = true;
00501     }
00502     else
00503         pc.printf   ("Not Fitted\r\n");
00504 
00505     uint32_t    old_hand_controller_direction = T5;
00506     if  (user_settings.rd(COMM_SRC) == 3)  {      //  Read fwd/rev switch 'T5', PA15 on 401
00507         pc.printf   ("Pot control\r\n");
00508         if  (T5)
00509             mode_set_motors_both   (MOTOR_FORWARD);  //  sets both motors
00510         else
00511             mode_set_motors_both   (MOTOR_REVERSE);
00512     }
00513 //    pc.printf   ("SystemCoreClock=%d, MAX_PWM_TICKS=%d\r\n", SystemCoreClock, MAX_PWM_TICKS);
00514 
00515     WatchDog    = WATCHDOG_RELOAD + 80; //  Allow extra few seconds at powerup
00516     pcc.flush   (); //  Flush serial rx buffers
00517     tsc.flush   ();
00518 //    pc.printf   ("sizeof int is %d\r\n", sizeof(int));  //  sizeof int is 4
00519     double      Current_Scaler;  //  New idea - Sept 2019. Plan is to scale down motor current limit when voltage lower than nom.
00520                                             //  See schematic for full details, but cycle-by-cycle current limit has the effect of allowing larger average I
00521                                             //  at lower voltages. This is simply because current takes longer to build in motor inductance when voltage
00522                                             //  is low. Conversely, at high supply voltages, motor current reaches limit quickly, cutting drive, meaning
00523                                             //  similar current flows for shorter times at higher voltages.
00524 
00525     while   (1) {      //  Loop forever, repeats synchroised by waiting for ticker Interrupt Service Routine to set 'loop_flag' true
00526         while   (!loop_flag)  {         //  Most of the time is spent in this loop, repeatedly re-checking for commands from pc port
00527             pcc.core    ()  ;   //  Proceed beyond here once loop_timer ticker ISR has set loop_flag true
00528             tsc.core    ()  ;   //  Proceed beyond here once loop_timer ticker ISR has set loop_flag true
00529             AtoD_reader ();                     //  Performs A to D conversions at rate set by ticker interrupts
00530         }                       //  32Hz original setting for loop repeat rate
00531         loop_flag = false;              //  Clear flag set by ticker interrupt handler. WHEN LAST CHECKED this was about every 32ms
00532 
00533             //  double  trim    (const double min, const double max, double input)  {
00534         Current_Scaler = trim  (0.1, 1.0, Read_BatteryVolts() / user_settings.Vnom());
00535         MotorA.I_scale  (Current_Scaler);   //  Reduces motor current limits when voltage below nominal.
00536         MotorB.I_scale  (Current_Scaler);   //  This goes some way towards preventing engine stalls perhaps
00537 
00538         MotorA.speed_monitor_and_control   ();   //  Needed to keep table updated to give reading in Hall transitions per second
00539         MotorB.speed_monitor_and_control   ();   //  Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM
00540 
00541         RC_chan_1.read(RCstick1);   //  Get latest info from Radio Control inputs
00542         RC_chan_2.read(RCstick2);
00543 
00544 //#define SERVO_OUT_TEST
00545 #ifdef  SERVO_OUT_TEST
00546         if  (RCstick1.active)   Servo1 = RCstick1.deflection;
00547         if  (RCstick2.active)   Servo2 = RCstick2.deflection;
00548 #endif
00549 
00550         switch  (user_settings.rd(COMM_SRC))  {   //  Look to selected source of driving command, act on commands from wherever
00551 //            case    0:  //  Invalid
00552 //                break;
00553 //            case    COM1:  //  COM1    -   Primarily for testing, bypassed by command line interpreter
00554 //                break;
00555             case    COM2:  //  COM2    -   Nothing done here, all serial instructions handled in command line interpreter
00556                 break;
00557             case    HAND:  //  Put all hand controller input stuff here
00558                 hand_control_state_machine  (HAND);
00559                 break;  //  endof hand controller stuff
00560             case    RC_IN1:  //  RC_chan_1
00561                 RC_chan_1.energise  (RCstick1, MotorA)  ;   //  RC chan 1 drives both motors
00562                 RC_chan_1.energise  (RCstick1, MotorB)  ;
00563                 break;
00564             case    RC_IN2:  //  RC_chan_2
00565                 RC_chan_2.energise  (RCstick2, MotorA)  ;   //  RC chan 2 drives both motors
00566                 RC_chan_2.energise  (RCstick2, MotorB)  ;
00567                 break;
00568             case    RC_IN_BOTH:  //  Robot Mode
00569                 RC_chan_1.energise  (RCstick1, MotorA)  ;   //  Two RC chans drive two motors independently
00570                 RC_chan_2.energise  (RCstick2, MotorB)  ;
00571                 break;
00572             default:
00573                 if  (ESC_Error.read(FAULT_UNRECOGNISED_STATE))  {
00574                     pc.printf   ("Unrecognised state %d\r\n", user_settings.rd(COMM_SRC));    //  set error flag instead here
00575                     ESC_Error.set   (FAULT_UNRECOGNISED_STATE, 1);
00576                 }
00577                 break;
00578         }   //  endof   switch  (user_settings_bytes[COMM_SRC])  {
00579 
00580 //#define SERVO_OUT_TEST
00581 //#ifdef  SERVO_OUT_TEST
00582 //    static double  servo_angle = 0.0;  //  For testing servo outs
00583         //  servo out test here     December 2018
00584 /*
00585         servo_angle += 0.01;
00586         if  (servo_angle > TWOPI)
00587             servo_angle -= TWOPI;
00588         Servo1 = ((sin(servo_angle)+1.0) / 2.0);
00589         Servo2 = ((cos(servo_angle)+1.0) / 2.0);
00590 */
00591         //  Yep!    Both servo outs work lovely December 2018
00592 //#endif
00593 
00594         if  (flag_8Hz)  {   //  do slower stuff
00595             flag_8Hz    = false;
00596             LED = !LED;                   // Toggle green LED on board, should be seen to fast flash
00597             if  (WatchDogEnable)    {
00598                 WatchDog--;
00599                 if  (WatchDog < 1) {   //  Deal with WatchDog timer timeout here
00600                     WatchDog = 0;
00601                     setVI_both  (0.0, 0.0);  //  set motor volts and amps to zero
00602                     pc.printf ("TIMEOUT %c\r\n", user_settings.rd(BOARD_ID));   //  Brute touch screen controller can do nothing with this
00603                 }                       //  End of dealing with WatchDog timer timeout
00604             }
00605             eighth_sec_count++;
00606             if  (eighth_sec_count > 6)    {   //  Send some status info out of serial port every second and a bit or thereabouts
00607                 eighth_sec_count = 0;
00608                 ESC_Error.report_any (false);   //  retain = false - reset error after reporting once
00609             }
00610         }   //  End of if(flag_8Hz)
00611     }       //  End of main programme loop
00612 }
00613