From dff51603288638b7672c1991d029ecc4fd739b29 Mon Sep 17 00:00:00 2001 From: Phil Malone Date: Sun, 13 Jun 2021 14:51:14 -0400 Subject: [PATCH] Added single serial port interface Added serial processing on debug Coms port. Accepts new Dual Speed command DSPE For left and right motor. --- Hoverboard/HUGS/Src/bldc.c | 21 ++-- Hoverboard/HUGS/Src/commsHUGS.c | 57 ++++++++-- Hoverboard/HUGS/Src/commsSteering.c | 168 +++++++++++++++++++++------- Hoverboard/HUGS/Src/it.c | 54 +++++---- Hoverboard/HUGS/Src/main.c | 86 +++++++------- Hoverboard/HUGS/Src/setup.c | 6 +- 6 files changed, 266 insertions(+), 126 deletions(-) diff --git a/Hoverboard/HUGS/Src/bldc.c b/Hoverboard/HUGS/Src/bldc.c index ad0b4a7..7fb4a98 100644 --- a/Hoverboard/HUGS/Src/bldc.c +++ b/Hoverboard/HUGS/Src/bldc.c @@ -36,16 +36,8 @@ #define PHASE_G_OFFSET (FULL_PHASE * 2 / 3) #define TRANSITION_ANGLE 233 -#define SPEED_MODE_PF 0 -#define SPEED_MODE_STEP 1 -#define SPEED_MODE_DUAL 2 -#define DEFAULT_SPEED_MODE SPEED_MODE_PF -#define DEFAULT_MAX_STEP_SPEED 200 - - // Internal constants const int16_t pwm_res = 72000000 / 2 / PWM_FREQ; // = 2000 - const int32_t WHEEL_PERIMETER = 530 ; // mm const int32_t SPEED_TICKS_FACTOR = 188444 ; // Divide factor by speed to get ticks per cycle, or visa versa. const int32_t SINE_TICKS_FACTOR = 3010 ; // Divide factor by speed to get ticks per degree. @@ -199,6 +191,11 @@ void SetEnable(FlagStatus setEnable) //---------------------------------------------------------------------------- void SetSpeed(int16_t speed) { + // Keep driver alive + if (abs16(speed) >= 10) { + resetInactivityTimer(); + } + closedLoopSpeed = TRUE; speedSetpoint = CLAMP(speed, -5000, 5000); @@ -237,6 +234,10 @@ void SetSpeed(int16_t speed) //---------------------------------------------------------------------------- void SetPower(int16_t power) { + // Keep driver alive + if (abs16(power) > 5) { + resetInactivityTimer(); + } closedLoopSpeed = FALSE; stepperMode = FALSE; speedSetpoint = 0; @@ -446,9 +447,9 @@ void CalculateBLDC(void) // PIDF coefficients are all scaled up by 15 bits (32768) const int32_t KF = (int32_t)(32768.0 * 0.16) ; -const int32_t KFO = (int32_t)(32768.0 * 14.0) ; // Kf Offset (to overcome motor deadband) was 28. try to reduce surge +const int32_t KFO = (int32_t)(32768.0 * 28.0) ; const int32_t KP = (int32_t)(32768.0 * 0.2) ; -const int32_t KI = (int32_t)(32768.0 * 0.00004) ; // was 0.00005 +const int32_t KI = (int32_t)(32768.0 * 0.00005) ; const int32_t ILIMIT = (int32_t)(32768.0 * 150) ; diff --git a/Hoverboard/HUGS/Src/commsHUGS.c b/Hoverboard/HUGS/Src/commsHUGS.c index b6a0caf..6117b36 100644 --- a/Hoverboard/HUGS/Src/commsHUGS.c +++ b/Hoverboard/HUGS/Src/commsHUGS.c @@ -64,10 +64,6 @@ void SendHUGSReply(void); uint16_t CalcCRC(uint8_t *ptr, int count); void ShutOff(void); - -typedef enum {NOP = 0, RSP, RES, ENA, DIS, POW, SPE, ABS, REL, DOG, MOD, XXX = 0xFF} CMD_ID; -typedef enum {NOR = 0, SMOT, SPOW, SSPE, SPOS, SVOL, SAMP, SDOG, SMOD, SFPI, STOP = 0xFF} RSP_ID; - // Variables updated by HUGS Message bool HUGS_ESTOP = FALSE; uint16_t HUGS_WatchDog = 1000 ; // TIMEOUT_MS; @@ -75,9 +71,12 @@ uint8_t HUGS_Destination = 0; uint8_t HUGS_Sequence = 0; CMD_ID HUGS_CommandID = NOP; RSP_ID HUGS_ResponseID = NOR; -bool HUGS_Enabled = FALSE; +void SetESTOP(void) { + HUGS_ESTOP = TRUE; +} + //---------------------------------------------------------------------------- // Update USART HUGS input //---------------------------------------------------------------------------- @@ -176,17 +175,14 @@ bool CheckUSARTHUGSInput(uint8_t USARTBuffer[]) switch(HUGS_CommandID) { case ENA: - HUGS_Enabled = TRUE; SetEnable(SET); break; case DIS: - HUGS_Enabled = FALSE; SetEnable(RESET); break; case POW: - HUGS_Enabled = TRUE; SetEnable(SET); SetPower((int16_t)((uint16_t)USARTBuffer[6] << 8) + (uint16_t)USARTBuffer[5]); break; @@ -209,7 +205,6 @@ bool CheckUSARTHUGSInput(uint8_t USARTBuffer[]) case SPE: // Set the constant Speed (in mm/s) - HUGS_Enabled = TRUE; SetEnable(SET); SetSpeed((int16_t)((uint16_t)USARTBuffer[6] << 8) + (uint16_t)USARTBuffer[5]); break; @@ -217,7 +212,7 @@ bool CheckUSARTHUGSInput(uint8_t USARTBuffer[]) case XXX: // powerdown SetPower(0); - HUGS_ESTOP = TRUE; + SetESTOP(); HUGS_ResponseID = STOP; break; @@ -356,3 +351,45 @@ void SendHUGSReply() SendBuffer(USART_HUGS, buffer, length + 8); } +//---------------------------------------------------------------------------- +// Send command via USART (Presumably to SLAVE) +//---------------------------------------------------------------------------- +void SendHUGSCmd(CMD_ID SlaveCmd, int16_t value) +{ + int8_t length = -1; + uint16_t crc = 0; + uint8_t buffer[USART_HUGS_TX_BYTES]; + + switch(SlaveCmd) { + case SPE: + length = 2; + buffer[5] = value & 0xFF ; + buffer[6] = value >> 8; + break; + + case XXX: + length = 0; + break; + + default: + break; + } + + // Only send a cmd if we have recognized the command and set a valid length + if (length >= 0) { + buffer[0] = '/'; + buffer[1] = length; + buffer[2] = 0; + buffer[3] = SlaveCmd; + buffer[4] = NOR; + + // Calculate CRC + crc = CalcCRC(buffer, length + 5); + buffer[length + 5] = crc & 0xFF; + buffer[length + 6] = (crc >> 8) & 0xFF; + buffer[length + 7] = '\n'; + + SendBuffer(USART_HUGS, buffer, length + 8); + } +} + diff --git a/Hoverboard/HUGS/Src/commsSteering.c b/Hoverboard/HUGS/Src/commsSteering.c index c01c29c..29a4eee 100644 --- a/Hoverboard/HUGS/Src/commsSteering.c +++ b/Hoverboard/HUGS/Src/commsSteering.c @@ -26,6 +26,7 @@ #include "gd32f1x0.h" #include "../Inc/it.h" #include "../Inc/comms.h" +#include "../Inc/commsHUGS.h" #include "../Inc/commsSteering.h" #include "../Inc/setup.h" #include "../Inc/config.h" @@ -34,32 +35,23 @@ #include "stdio.h" #include "string.h" -// Only master communicates with steerin device -#ifdef MASTER -#define USART_STEER_TX_BYTES 2 // Transmit byte count including start '/' and stop character '\n' -#define USART_STEER_RX_BYTES 8 // Receive byte count including start '/' and stop character '\n' +#define STEER_MAX_DATA 4 // Max variable Data Length' +#define STEER_EOM_OFFSET 7 // Location of EOM char based on variable data length +#define USART_STEER_RX_BYTES (STEER_MAX_DATA + 8) // start '/' and stop character '\n' +#define USART_STEER_TX_BYTES 2 // Max buffeer size including extern uint8_t usartSteer_COM_rx_buf[USART_STEER_COM_RX_BUFFERSIZE]; -static uint8_t sSteerRecord = 0; + +static bool sSteerRecord = FALSE; static uint8_t sUSARTSteerRecordBuffer[USART_STEER_RX_BYTES]; static uint8_t sUSARTSteerRecordBufferCounter = 0; +static uint8_t steerReply[USART_STEER_TX_BYTES] = {'/', '\n'}; -void CheckUSARTSteerInput(uint8_t u8USARTBuffer[]); +CMD_ID Steer_CommandID = NOP; +RSP_ID Steer_ResponseID = NOR; + +bool CheckUSARTSteerInput(uint8_t u8USARTBuffer[]); -//---------------------------------------------------------------------------- -// Send frame to steer device -//---------------------------------------------------------------------------- -void SendSteerDevice(void) -{ - int index = 0; - uint8_t buffer[USART_STEER_TX_BYTES]; - - // Ask for steer input - buffer[index++] = '/'; - buffer[index++] = '\n'; - - SendBuffer(USART_STEER_COM, buffer, index); -} //---------------------------------------------------------------------------- // Update USART steer input @@ -67,59 +59,153 @@ void SendSteerDevice(void) void UpdateUSARTSteerInput(void) { uint8_t character = usartSteer_COM_rx_buf[0]; + uint8_t length; + // Start character is captured, start record - if (character == '/') + if (!sSteerRecord && (character == '/')) { sUSARTSteerRecordBufferCounter = 0; - sSteerRecord = 1; + sSteerRecord = TRUE; } + // Process the new charcter if (sSteerRecord) { sUSARTSteerRecordBuffer[sUSARTSteerRecordBufferCounter] = character; sUSARTSteerRecordBufferCounter++; - if (sUSARTSteerRecordBufferCounter >= USART_STEER_RX_BYTES) - { - sUSARTSteerRecordBufferCounter = 0; - sSteerRecord = 0; + // Check to see if we know the length yet + if (sUSARTSteerRecordBufferCounter > 1) { - // Check input - CheckUSARTSteerInput (sUSARTSteerRecordBuffer); + // Check for an invalid length, or a completed message + if ((length = sUSARTSteerRecordBuffer[1]) > STEER_MAX_DATA){ + // Bad data length + sUSARTSteerRecordBufferCounter = 0; + sSteerRecord = FALSE; + } + else if (sUSARTSteerRecordBufferCounter > (length + STEER_EOM_OFFSET)) + { + + // Check input using HUGS message processing + if (CheckUSARTSteerInput (sUSARTSteerRecordBuffer)) { + + // FOR TEST PURPOSES + // SendBuffer(USART_STEER_COM, sUSARTSteerRecordBuffer, sUSARTSteerRecordBufferCounter); + + // A complete message was found. Reset buffer and status + sUSARTSteerRecordBufferCounter = 0; + sSteerRecord = FALSE; + } else { + // Message was invalid. it could have been a bad SOM + // check to see if the buffer holds another SOM (/) + int slider = 0; + int ch; + + for (ch = 1; ch < sUSARTSteerRecordBufferCounter; ch++) { + if (sUSARTSteerRecordBuffer[ch] == '/') { + slider = ch; + break; + } + } + + if (slider > 0) { + // push the buffer back + sUSARTSteerRecordBufferCounter -= slider; + memcpy(sUSARTSteerRecordBuffer, sUSARTSteerRecordBuffer + slider, sUSARTSteerRecordBufferCounter); + } else { + sUSARTSteerRecordBufferCounter = 0; + sSteerRecord = FALSE; + } + } + } } } + } //---------------------------------------------------------------------------- // Check USART steer input //---------------------------------------------------------------------------- -void CheckUSARTSteerInput(uint8_t USARTBuffer[]) +bool CheckUSARTSteerInput(uint8_t USARTBuffer[]) { // Auxiliary variables - uint16_t crc; + uint16_t crc; + uint8_t length = USARTBuffer[1]; + int16_t turn_mmPS; + int16_t left_mmPS; + int16_t right_mmPS; + int16_t smax; + // Check start and stop character if ( USARTBuffer[0] != '/' || - USARTBuffer[USART_STEER_RX_BYTES - 1] != '\n') + USARTBuffer[length + STEER_EOM_OFFSET ] != '\n') { - return; + return FALSE; } - - // Calculate CRC (first bytes except crc and stop byte) - crc = CalcCRC(USARTBuffer, USART_STEER_RX_BYTES - 3); + + // Calculate CRC (first bytes up to, not including crc) + crc = CalcCRC(USARTBuffer, length + 5 ); // Check CRC - if ( USARTBuffer[USART_STEER_RX_BYTES - 3] != ((crc >> 8) & 0xFF) || - USARTBuffer[USART_STEER_RX_BYTES - 2] != (crc & 0xFF)) + if ( USARTBuffer[length + 5] != (crc & 0xFF) || + USARTBuffer[length + 6] != ((crc >> 8) & 0xFF) ) { - return; + return FALSE; } - // Calculate result speed value -1000 to 1000 - // speed = (int16_t)((USARTBuffer[1] << 8) | USARTBuffer[2]); + // command is valid. Process it now + Steer_CommandID = (CMD_ID)USARTBuffer[3] ; + Steer_ResponseID = (RSP_ID)USARTBuffer[4] ; + + switch(Steer_CommandID) { + case DSPE: + + // Set the constant Speed (in mm/s) + SetEnable(SET); + + left_mmPS = (int16_t)((uint16_t)USARTBuffer[6] << 8) + (uint16_t)USARTBuffer[5]; + right_mmPS = left_mmPS; + + turn_mmPS = (int16_t)((float)((int16_t)((uint16_t)USARTBuffer[8] << 8) + (uint16_t)USARTBuffer[7]) * MM_PER_DEGREE); + + left_mmPS += turn_mmPS; + right_mmPS -= turn_mmPS; + + // normalize so no speed is > +/-TOP_SPEED + smax = max(abs16(left_mmPS), abs16(right_mmPS)); + + if (smax > MAX_SPEED) { + left_mmPS = (left_mmPS * MAX_SPEED) / smax; + right_mmPS = (right_mmPS * MAX_SPEED) / smax; + } + + SendHUGSCmd(SPE, -left_mmPS) ; + SetSpeed(right_mmPS); + break; + + case XXX: + // powerdown + SendHUGSCmd(SPE, 0) ; + SendHUGSCmd(XXX, 0) ; + SetPower(0); + SetESTOP(); + break; + + default: + break; + } + + // Send keep-alive reply + SendBuffer(USART_STEER_COM, steerReply, sizeof(steerReply)); // Reset the pwm timout to avoid stopping motors ResetTimeout(); + + return TRUE; +} + +int16_t max(int16_t a, int16_t b){ + return (a >= b)? a : b; } -#endif diff --git a/Hoverboard/HUGS/Src/it.c b/Hoverboard/HUGS/Src/it.c index 9bf05d3..48c0142 100644 --- a/Hoverboard/HUGS/Src/it.c +++ b/Hoverboard/HUGS/Src/it.c @@ -32,53 +32,61 @@ #include "../Inc/commsHUGS.h" #include "../Inc/commsSteering.h" -uint32_t msTicks; -uint32_t timeoutCounter_ms = 0; -FlagStatus timedOut = SET; +uint32_t msTicks = 0; +uint32_t sysTicks = 0; +uint32_t driveSafeMs = 0; +FlagStatus timedOut = SET; +bool msToggle = FALSE; extern FlagStatus activateWeakening; extern int16_t HUGS_WatchDog; //---------------------------------------------------------------------------- -// SysTick_Handler +// SysTick_Handler (Fires every 10 mS) //---------------------------------------------------------------------------- void SysTick_Handler(void) { - msTicks++; + sysTicks++; } //---------------------------------------------------------------------------- -// Resets the timeout to zero +// Resets the drive timeout to zero //---------------------------------------------------------------------------- void ResetTimeout(void) { - timeoutCounter_ms = 0; + driveSafeMs = 0; } //---------------------------------------------------------------------------- // Timer13_Update_Handler // Is called when upcouting of timer13 is finished and the UPDATE-flag is set -// -> period of timer13 running with 1kHz -> interrupt every 1ms (actually 500 uS) +// -> period of timer13 running with 2kHz -> interrupt every 500 uS //---------------------------------------------------------------------------- void TIMER13_IRQHandler(void) { - // Update speed value - CalculateSpeed(); + msToggle = msToggle ? FALSE : TRUE; - if (timeoutCounter_ms > (HUGS_WatchDog << 1)) - { - // First timeout reset all process values - if (timedOut == RESET) + if (msToggle) { + msTicks++; + + // Update speed value + CalculateSpeed(); + + if (driveSafeMs > HUGS_WatchDog) { - SetPower(0); + // First timeout reset all process values + if (timedOut == RESET) + { + SetPower(0); + } + + timedOut = SET; + } + else + { + timedOut = RESET; + driveSafeMs++; } - - timedOut = SET; - } - else - { - timedOut = RESET; - timeoutCounter_ms++; } // Clear timer update interrupt flag @@ -162,7 +170,7 @@ uint32_t millis() } //---------------------------------------------------------------------------- -// Delays number of tick Systicks (happens every 10 ms) +// Delays number of mSec //---------------------------------------------------------------------------- void Delay (uint32_t dlyTicks) { diff --git a/Hoverboard/HUGS/Src/main.c b/Hoverboard/HUGS/Src/main.c index 0727cc6..4e931a5 100644 --- a/Hoverboard/HUGS/Src/main.c +++ b/Hoverboard/HUGS/Src/main.c @@ -38,6 +38,7 @@ #include "../Inc/config.h" #include "../Inc/it.h" #include "../Inc/bldc.h" +#include "../Inc/comms.h" #include "../Inc/commsHUGS.h" #include "../Inc/commsSteering.h" #include "stdio.h" @@ -45,27 +46,21 @@ #include "string.h" #include -#ifdef MASTER FlagStatus activateWeakening = RESET; // global variable for weakening extern uint16_t batteryVoltagemV; // global variable for battery voltage extern FlagStatus timedOut; // Timeoutvariable set by timeout timer -extern bool HUGS_Enabled; // Set by HUGS communications extern bool HUGS_ESTOP; -extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7... +extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7... extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7... - -uint16_t command_timeout_counter = 0; // motor safety counter ms From last command -uint32_t inactivity_timeout_counter = 0; // Inactivity counter ms From last command +uint32_t inactivityTimer = 0; // Inactivity counter ms From last command void ShowBatteryState(uint32_t pin); void ShutOff(void); -#endif - //---------------------------------------------------------------------------- // MAIN function @@ -111,9 +106,9 @@ int main (void) // Init usart steer/bluetooth USART_Steer_COM_init(); - // Startup-Sound + // Startup-Sound. This plays for 1 second buzzerFreq = 7; - Delay(100); + Delay(1000); fwdgt_counter_reload(); buzzerFreq = 0; @@ -123,7 +118,7 @@ int main (void) // Reload watchdog while button is pressed fwdgt_counter_reload(); } - + while(1) { // Shut device if ESTOP requested or when button is pressed @@ -132,34 +127,44 @@ int main (void) ShutOff(); } - // Show green battery symbol when battery level BAT_LOW_LVL1 is reached - if (batteryVoltagemV > BAT_LOW_LVL1_MV) - { - // Show green battery light - ShowBatteryState(LED_GREEN); - } - // Make silent sound and show orange battery symbol when battery level BAT_LOW_LVL2 is reached - else if (batteryVoltagemV > BAT_LOW_LVL2_MV && batteryVoltagemV < BAT_LOW_LVL1_MV) - { - // Show orange battery light - ShowBatteryState(LED_ORANGE); - } - // Make even more sound and show red battery symbol when battery level BAT_LOW_DEAD is reached - else if (batteryVoltagemV > BAT_LOW_DEAD_MV && batteryVoltagemV < BAT_LOW_LVL2_MV) - { - // Show red battery light - ShowBatteryState(LED_RED); - } - else - { - ShutOff(); - } - // Shut off device after INACTIVITY_TIMEOUT in minutes - if (inactivity_timeout_counter++ > (INACTIVITY_TIMEOUT * 60 * 1000) / (DELAY_IN_MAIN_LOOP + 1)) + if (inactivityTimer++ > INACTIVITY_COUNTER) { ShutOff(); - } + } else if (inactivityTimer > INACTIVITY_WARNING) { + buzzerFreq = 8; + buzzerPattern = 8; + } else { + + // Show green battery symbol when battery level BAT_LOW_LVL1 is reached + if (batteryVoltagemV > BAT_LOW_LVL1_MV) + { + // Show green battery light + ShowBatteryState(LED_GREEN); + buzzerFreq = 0; + } + // Make silent sound and show orange battery symbol when battery level BAT_LOW_LVL2 is reached + else if (batteryVoltagemV > BAT_LOW_LVL2_MV && batteryVoltagemV < BAT_LOW_LVL1_MV) + { + // Show orange battery light + ShowBatteryState(LED_ORANGE); + buzzerFreq = 5; + buzzerPattern = 8; + } + // Make even more sound and show red battery symbol when battery level BAT_LOW_DEAD is reached + else if (batteryVoltagemV > BAT_LOW_DEAD_MV && batteryVoltagemV < BAT_LOW_LVL2_MV) + { + // Show red battery light + ShowBatteryState(LED_RED); + buzzerFreq = 5; + buzzerPattern = 1; + } + else + { + ShutOff(); + } + } + Delay(DELAY_IN_MAIN_LOOP); @@ -181,16 +186,17 @@ void ShutOff(void) // Ensure that drive is off and estop status set. SetPWM(0); - HUGS_Enabled = FALSE; HUGS_ESTOP = TRUE; SetEnable(RESET); SendHUGSReply(); // Transfer ESTOP to Controller + SendHUGSCmd(XXX, 0); // Tell possible slave to stop as well. + // Play shutdown sound for (index = 0; index < 8; index++) { buzzerFreq = index; - Delay(10); + Delay(100); } buzzerFreq = 0; @@ -216,3 +222,7 @@ void ShowBatteryState(uint32_t pin) gpio_bit_write(LED_RED_PORT, LED_RED, pin == LED_RED ? SET : RESET); } + +void resetInactivityTimer(void) { + inactivityTimer = 0; +} diff --git a/Hoverboard/HUGS/Src/setup.c b/Hoverboard/HUGS/Src/setup.c index fdb732a..430cb78 100644 --- a/Hoverboard/HUGS/Src/setup.c +++ b/Hoverboard/HUGS/Src/setup.c @@ -209,7 +209,6 @@ void GPIO_init(void) gpio_af_set(USART_STEER_COM_TX_PORT, GPIO_AF_0, USART_STEER_COM_TX_PIN); gpio_af_set(USART_STEER_COM_RX_PORT, GPIO_AF_0, USART_STEER_COM_RX_PIN); -#ifdef MASTER // Init buzzer gpio_mode_set(BUZZER_PORT , GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, BUZZER_PIN); gpio_output_options_set(BUZZER_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, BUZZER_PIN); @@ -219,7 +218,6 @@ void GPIO_init(void) // Init charge state gpio_mode_set(CHARGE_STATE_PORT, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, CHARGE_STATE_PIN); -#endif } //---------------------------------------------------------------------------- @@ -444,8 +442,8 @@ void USART_Steer_COM_init(void) rcu_periph_clock_enable(RCU_USART0); rcu_periph_clock_enable(RCU_DMA); - // Init USART for 19200 baud, 8N1 - usart_baudrate_set(USART_STEER_COM, 19200); + // Init USART for 38400 baud, 8N1 (Works best with PIC 8MHz) + usart_baudrate_set(USART_STEER_COM, 38400); usart_parity_config(USART_STEER_COM, USART_PM_NONE); usart_word_length_set(USART_STEER_COM, USART_WL_8BIT); usart_stop_bit_set(USART_STEER_COM, USART_STB_1BIT);