Skip to content

Commit

Permalink
Added single serial port interface
Browse files Browse the repository at this point in the history 
Added serial processing on debug Coms port.
Accepts new Dual Speed command DSPE  For left and right motor.
  • Loading branch information
@gearsincorg
gearsincorg committed Jun 13, 2021
1 parent d4217de commit dff5160
Show file tree
Hide file tree
Showing 6 changed files with 266 additions and 126 deletions.
21 changes: 11 additions & 10 deletions Hoverboard/HUGS/Src/bldc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) ;


Expand Down
57 changes: 47 additions & 10 deletions Hoverboard/HUGS/Src/commsHUGS.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,20 +64,19 @@ 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;
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
//----------------------------------------------------------------------------
Expand Down Expand Up @@ -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;
Expand All @@ -209,15 +205,14 @@ 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;

case XXX:
// powerdown
SetPower(0);
HUGS_ESTOP = TRUE;
SetESTOP();
HUGS_ResponseID = STOP;
break;

Expand Down Expand Up @@ -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);
}
}

168 changes: 127 additions & 41 deletions Hoverboard/HUGS/Src/commsSteering.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -34,92 +35,177 @@
#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
//----------------------------------------------------------------------------
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
Loading

0 comments on commit dff5160

Please sign in to comment.