STM3 ESC dual brushless motor controller. 10-60v, motor power rating tiny to kW. Ganged or independent motor control As used in 'The Brute' locomotive - www.jons-workshop.com
Dependencies: mbed BufferedSerial Servo FastPWM
main.cpp
- Committer:
- JonFreeman
- Date:
- 2019-03-04
- Revision:
- 12:d1d21a2941ef
- Parent:
- 11:bfb73f083009
File content as of revision 12:d1d21a2941ef:
// 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 "BufferedSerial.h" #include "FastPWM.h" #include "Servo.h" #include "brushless_motor.h" #include "Radio_Control_In.h" /* Brushless_STM3 board Jan 2019 * WatchDog implemented. Default is disabled, 'kd' command from TS controller enables and reloads * Tidied brushless_motor class, parameter passing now done properly * class RControl_In created. Inputs now routed to new pins, can now use two chans both class RControl_In and Servo out (buggery board required for new inputs) * Added version string * Error handler written and included * 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 * Reorganised EEPROM data - mode setting now much easier, less error prone * Maximum speed now one EEPROM option, range 1.0 to 25.0 MPH 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 */ /* STM32F401RE - compile using NUCLEO-F401RE // PROJECT - STM3_ESC Dual Brushless Motor Controller - Jon Freeman June 2018. AnalogIn to read each motor current ____________________________________________________________________________________ CONTROL PHILOSOPHY This Bogie drive board software should ensure sensible control when commands supplied are not sensible ! That is, smooth transition between Drive, Coast and Brake to be implemented here. The remote controller must not be relied upon to deliver sensible command sequences. So much the better if the remote controller does issue sensible commands, but ... ____________________________________________________________________________________ */ #if defined (TARGET_NUCLEO_F401RE) // CPU in 64 pin LQFP #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" // A thought for future version #endif /* Global variable declarations */ char const const_version_string[] = {"1.0.0\0"}; // Version string, readable from serial ports volatile uint32_t fast_sys_timer = 0; // gets incremented by our Ticker ISR every VOLTAGE_READ_INTERVAL_US int WatchDog = WATCHDOG_RELOAD + 80; // Allow extra few seconds at powerup bool WatchDogEnable = false; // Must recieve explicit instruction from pc or controller to enable uint32_t volt_reading = 0, // Global updated by interrupt driven read of Battery Volts driverpot_reading = 0, // Global updated by interrupt driven read of Drivers Pot sys_timer = 0, // gets incremented by our Ticker ISR every MAIN_LOOP_REPEAT_TIME_US AtoD_Semaphore = 0; 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; double rpm2mph; uint32_t temp_sensor_count = 0, // incremented by every rising edge from LMT01 last_temperature_count = 0; // global updated approx every 100ms after each LMT01 conversion completes /* End of Global variable declarations */ Ticker tick_vread; // Device to cause periodic interrupts, used to time voltage readings etc Ticker loop_timer; // Device to cause periodic interrupts, used to sync iterations of main programme loop Ticker temperature_find_ticker; Timer temperature_timer; #ifdef USING_DC_MOTORS Timer dc_motor_kicker_timer; Timeout motors_restore; #endif 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 error_handling_Jan_2019 ESC_Error ; // Provides array usable to store error codes. eeprom_settings mode (SDA_PIN, SCL_PIN) ; // This MUST come before Motors setup //uint32_t HAtot = 0, HBtot = 0, A_Offset = 0, B_Offset = 0; /* 5 1 3 2 6 4 SENSOR SEQUENCE 1 1 1 1 0 0 0 ---___---___ Hall1 2 0 0 1 1 1 0 __---___---_ Hall2 4 1 0 0 0 1 1 -___---___-- Hall3 UV WV WU VU VW UW OUTPUT SEQUENCE 8th July 2018 Added drive to DC brushed motors. Connect U and W to dc motor, leave V open. Hall codes 0 and 7 not used for brushless motors. Without Hall sensors connected pullup resistors give code 7. Use this for dc motors */ const uint16_t A_tabl[] = { // Origial table 0, 0, 0, 0, 0, 0, 0, 0, // Handbrake 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] } ; 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 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 } ; brushless_motor MotorA (MOT_A_I_ADC, APWMV, APWMI, A_tabl, _MAH1, _MAH2, _MAH3, PortA, PORT_A_MASK, ISHUNTA); brushless_motor MotorB (MOT_B_I_ADC, BPWMV, BPWMI, B_tabl, _MBH1, _MBH2, _MBH3, PortB, PORT_B_MASK, ISHUNTB); extern cli_2019 pcc, tsc; // command line interpreters from pc and touch screen // Interrupt Service Routines void ISR_temperature_find_ticker () // every 960 us, i.e. slightly faster than once per milli sec { static bool readflag = false; int t = temperature_timer.read_ms (); if ((t == 5) && (!readflag)) { last_temperature_count = temp_sensor_count; temp_sensor_count = 0; readflag = true; } if (t == 6) readflag = false; } /** void ISR_loop_timer () * This ISR responds to Ticker interrupts at a rate of (probably) 32 times per second (check from constant declarations above) * This ISR sets global flag 'loop_flag' used to synchronise passes around main programme control loop. * Increments global 'sys_timer', usable anywhere as general measure of elapsed time */ void ISR_loop_timer () // This is Ticker Interrupt Service Routine - loop timer - MAIN_LOOP_REPEAT_TIME_US { loop_flag = true; // set flag to allow main programme loop to proceed sys_timer++; // Just a handy measure of elapsed time for anything to use if ((sys_timer & 0x03) == 0) flag_8Hz = true; } /** void ISR_voltage_reader () * This ISR responds to Ticker interrupts every 'VOLTAGE_READ_INTERVAL_US' micro seconds * * AtoD_reader() called from convenient point in code to take readings outside of ISRs */ void ISR_voltage_reader () // This is Ticker Interrupt Service Routine ; few us between readings ; VOLTAGE_READ_INTERVAL_US = 50 { AtoD_Semaphore++; fast_sys_timer++; // Just a handy measure of elapsed time for anything to use } 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 } // End of Interrupt Service Routines void setVI (double v, double i) { MotorA.set_V_limit (v); // Sets max motor voltage MotorA.set_I_limit (i); // Sets max motor current MotorB.set_V_limit (v); MotorB.set_I_limit (i); } /** * void AtoD_reader () // Call to here every VOLTAGE_READ_INTERVAL_US = 50 once loop responds to flag set in isr * Not part of ISR */ 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; if (MotorA.tickleon) MotorA.high_side_off (); if (MotorB.tickleon) MotorB.high_side_off (); if (AtoD_Semaphore > 20) { pc.printf ("WARNING - sema cnt %d\r\n", AtoD_Semaphore); AtoD_Semaphore = 20; } while (AtoD_Semaphore > 0) { AtoD_Semaphore--; // Code here to sequence through reading voltmeter, demand pot, ammeters switch (i) { // case 0: volt_reading += Ain_SystemVolts.read_u16 (); // Result = Result + New Reading volt_reading >>= 1; // Result = Result / 2 break; // i.e. Very simple digital low pass filter case 1: driverpot_reading += Ain_DriverPot.read_u16 (); driverpot_reading >>= 1; break; case 2: MotorA.sniff_current (); // Initiate ADC current reading break; case 3: MotorB.sniff_current (); break; } i++; // prepare to read the next input in response to the next interrupt if (i > 3) i = 0; } // end of while (AtoD_Semaphore > 0) { if (MotorA.tickleon) { MotorA.tickleon--; MotorA.motor_set (); // Reactivate any high side switches turned off above } if (MotorB.tickleon) { MotorB.tickleon--; MotorB.motor_set (); } } /** 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 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 () { return ((double) driverpot_reading) / 65536.0; // Normalise 0.0 <= control pot <= 1.0 } double Read_BatteryVolts () { return ((double) volt_reading) / 951.0; // divisor fiddled to make voltage reading correct ! } void mode_set_both_motors (int mode, double val) // called from cli to set fw, re, rb, hb { MotorA.set_mode (mode); MotorB.set_mode (mode); if (mode == MOTOR_REGENBRAKE) { if (val > 1.0) val = 1.0; if (val < 0.0) val = 0.0; val *= 0.9; // set upper limit, this is essential val = sqrt (val); // to linearise effect setVI (val, 1.0); } } #ifdef USING_DC_MOTORS void restorer () { motors_restore.detach (); if (MotorA.dc_motor) { MotorA.set_V_limit (MotorA.last_V); MotorA.set_I_limit (MotorA.last_I); MotorA.motor_set (); } if (MotorB.dc_motor) { MotorB.set_V_limit (MotorB.last_V); MotorB.set_I_limit (MotorB.last_I); MotorB.motor_set (); } } #endif 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()); } 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; 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; 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; } switch (hand_controller_state) { case HAND_CONT_IDLE: // Here for first few passes until validated direction switch reading break; 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 (MOTOR_REGENBRAKE, brake_effort); // Direction change hand_controller_state = HAND_CONT_BRAKE_POT; break; 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 (MOTOR_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 if (pot_position < 0.02) { // Driver has turned pot fully anticlock hand_controller_state = HAND_CONT_STATE_BRAKING; } } 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 (MOTOR_REGENBRAKE, brake_effort); old_pot_position = pot_position; hand_controller_state = HAND_CONT_STATE_BRAKING; pc.printf ("Brake\r\n"); break; case HAND_CONT_STATE_BRAKING: if (pot_position > Pot_Brake_Range) // escape braking into drive hand_controller_state = HAND_CONT_STATE_INTO_DRIVING; else { if (worth_the_bother(pot_position, old_pot_position)) { old_pot_position = pot_position; brake_effort = (Pot_Brake_Range - pot_position) / Pot_Brake_Range; mode_set_both_motors (MOTOR_REGENBRAKE, brake_effort); // pc.printf ("Brake %.2f\r\n", brake_effort); } } break; 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 (MOTOR_FORWARD, 0.0); // sets both motors else mode_set_both_motors (MOTOR_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 hand_controller_state = HAND_CONT_STATE_INTO_BRAKING; else { if (worth_the_bother(pot_position, old_pot_position)) { old_pot_position = pot_position; drive_effort = (pot_position - Pot_Brake_Range) / (1.0 - Pot_Brake_Range); setVI (drive_effort, drive_effort); // Wind volts and amps up and down together ??? pc.printf ("Drive %.2f\r\n", drive_effort); } } break; default: pc.printf ("Unhandled Hand Controller state %d\r\n", hand_controller_state); break; } // endof switch (hand_controller_state) { } int main() { int eighth_sec_count = 0; double servo_angle = 0.0; // For testing servo outs Temperature_pin.fall (&temp_sensor_isr); Temperature_pin.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 temperature_find_ticker.attach_us (&ISR_temperature_find_ticker, 960); // Done setting up system interrupt timers temperature_timer.start (); pc.baud (9600); // COM port to pc com3.baud (1200); // Once had an idea to use this for IR comms, never tried com2.baud (19200); // Opto isolated serial bus connecting 'n' STM3_ESC boards to 1 Brute Touch Screen controller rpm2mph = 60.0 // to Motor Revs per hour; * ((double)mode.rd(MOTPIN) / (double)mode.rd(WHEELGEAR)) // Wheel revs per hour * PI * ((double)mode.rd(WHEELDIA) / 1000.0) // metres per hour * 39.37 / (1760.0 * 36.0); // miles per hour MotorA.direction_set (mode.rd(MOTADIR)); // modes all setup in class eeprom_settings {} mode ; constructor MotorB.direction_set (mode.rd(MOTBDIR)); MotorA.poles (mode.rd(MOTAPOLES)); // Returns true if poles 4, 6 or 8. Returns false otherwise MotorB.poles (mode.rd(MOTBPOLES)); // Only two calls are here MotorA.set_mode (MOTOR_REGENBRAKE); MotorB.set_mode (MOTOR_REGENBRAKE); setVI (0.9, 0.5); // Power up with moderate regen braking applied // T1 = 0; Now WRONGLY hoped to be InterruptIn counting pulses from LMT01 temperature sensor T2 = 0; // T2, T3, T4 As yet unused pins // T3 = 0; // T4 = 0; // T5 = 0; now input from fw/re on remote control box T6 = 0; if ((last_temperature_count > 160) && (last_temperature_count < 2400)) // in range -40 to +100 degree C temp_sensor_exists = true; #ifdef USING_DC_MOTORS dc_motor_kicker_timer.start (); #endif int old_hand_controller_direction = T5; if (mode.rd(COMM_SRC) == 3) { // Read fwd/rev switch 'T5', PA15 on 401 pc.printf ("Pot control\r\n"); if (T5) mode_set_both_motors (MOTOR_FORWARD, 0.0); // sets both motors else mode_set_both_motors (MOTOR_REVERSE, 0.0); } pc.printf ("About to start %s!, mode_bytes[COMM_SRC]= %d\r\n", const_version_string, mode.rd(COMM_SRC)); pc.printf ("ESC_Error.all_good() ret'd %s\r\n", ESC_Error.all_good() ? "true" : "false"); // pc.printf ("SystemCoreClock=%d, MAX_PWM_TICKS=%d\r\n", SystemCoreClock, MAX_PWM_TICKS); // pcc.test () ; // tsc.test () ; 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 pcc.core () ; // Proceed beyond here once loop_timer ticker ISR has set loop_flag true tsc.core () ; // 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(); RC_chan_2.validate_rx(); switch (mode.rd(COMM_SRC)) { // Look to selected source of driving command, act on commands from wherever case 0: // Invalid break; case COM1: // COM1 - Primarily for testing, bypassed by command line interpreter break; case COM2: // COM2 - Primarily for testing, bypassed by command line interpreter break; case HAND: // Put all hand controller input stuff here hand_control_state_machine (3); break; // endof hand controller stuff case RC_IN1: // RC_chan_1 hand_control_state_machine (4); break; case RC_IN2: // RC_chan_2 break; default: if (ESC_Error.read(FAULT_UNRECOGNISED_STATE)) { pc.printf ("Unrecognised state %d\r\n", mode.rd(COMM_SRC)); // set error flag instead here ESC_Error.set (FAULT_UNRECOGNISED_STATE, 1); } break; } // endof switch (mode_bytes[COMM_SRC]) { #ifdef USING_DC_MOTORS dc_motor_kicker_timer.reset (); #endif MotorA.speed_monitor_and_control (); // Needed to keep table updated to give reading in Hall transitions per second MotorB.speed_monitor_and_control (); // Read MotorX.PPS to read pulses per sec or MotorX.RPM to read motor RPM #ifdef USING_DC_MOTORS if (MotorA.dc_motor) { // MotorA.raw_V_pwm (1); MotorA.low_side_on (); } if (MotorB.dc_motor) { // MotorB.raw_V_pwm (1); MotorB.low_side_on (); } if (MotorA.dc_motor || MotorB.dc_motor) { // motors_restore.attach_us (&restorer, ttime); motors_restore.attach_us (&restorer, 25); } #endif if (flag_8Hz) { // do slower stuff flag_8Hz = false; LED = !LED; // Toggle LED on board, should be seen to fast flash if (WatchDogEnable) { WatchDog--; if (WatchDog == 0) { // Deal with WatchDog timer timeout here setVI (0.0, 0.0); // set motor volts and amps to zero // com2.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Potential problem of multiple units reporting at same time overcome by adding board number to WATCHDOG_RELOAD pc.printf ("TIMEOUT %c\r\n", mode.rd(BOARD_ID)); // Brute touch screen controller can do nothing with this } // End of dealing with WatchDog timer timeout if (WatchDog < 0) WatchDog = 0; } eighth_sec_count++; if (eighth_sec_count > 6) { // Send some status info out of serial port every second and a bit or thereabouts eighth_sec_count = 0; ESC_Error.report_any (false); // retain = false - reset error after reporting once /* if (temp_sensor_exists) { double tmprt = (double) last_temp_count; tmprt /= 16.0; tmprt -= 50.0; pc.printf ("Temp %.2f\r\n", tmprt); }*/ } #define SERVO_OUT_TEST #ifdef SERVO_OUT_TEST // 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 #endif } // End of if(flag_8Hz) } // End of main programme loop }