bobbycar/controller_teensy/src/main.cpp

192 lines
6.2 KiB
C++
Raw Normal View History

#include <Arduino.h>
// ########################## DEFINES ##########################
2021-03-02 22:48:20 +00:00
#define SERIAL_CONTROL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
2021-03-02 22:48:20 +00:00
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
2021-03-02 22:48:20 +00:00
#define RECEIVEPERIOD 50 //ms
#define PIN_THROTTLE A0
const uint16_t calib_throttle_min = 350;
const uint16_t calib_throttle_max = 810;
int16_t out_speedFL=0;
int16_t out_speedFR=0;
int16_t out_speedRL=0;
int16_t out_speedRR=0;
unsigned long last_send = 0;
2021-03-02 22:48:20 +00:00
unsigned long last_receive = 0;
// Global variables for serial communication
typedef struct{
uint8_t idx = 0; // Index for new data pointer
uint16_t bufStartFrame; // Buffer Start Frame
byte *p; // Pointer declaration for the new received data
byte incomingByte;
byte incomingBytePrev;
long lastValidDataSerial_time;
} SerialRead;
SerialRead SerialcomFront;
typedef struct{
uint16_t start;
int16_t speedLeft;
int16_t speedRight;
uint16_t checksum;
} SerialCommand;
SerialCommand CommandFront;
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedL_meas;
int16_t speedR_meas;
int16_t batVoltage;
int16_t boardTemp;
int16_t curL_DC;
int16_t curR_DC;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
SerialFeedback FeedbackFront;
SerialFeedback NewFeedbackFront;
2021-03-02 22:48:20 +00:00
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
{
// Create command
scom.start = (uint16_t)START_FRAME;
scom.speedLeft = (int16_t)uSpeedLeft;
scom.speedRight = (int16_t)uSpeedRight;
scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
2021-03-02 22:48:20 +00:00
SerialRef.write((uint8_t *) &scom, sizeof(scom));
}
2021-03-02 22:48:20 +00:00
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
{
2021-03-02 22:48:20 +00:00
bool _result=1;
// Check for new data availability in the Serial buffer
2021-03-02 22:48:20 +00:00
if ( SerialRef.available() ) {
sread.incomingByte = SerialRef.read(); // Read the incoming byte
sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame
}
else {
2021-03-02 22:48:20 +00:00
return 0;
}
// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
Serial.print(sread.incomingByte);
#endif
// Copy received data
if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected
sread.p = (byte *)&NewFeedback;
*sread.p++ = sread.incomingBytePrev;
*sread.p++ = sread.incomingByte;
sread.idx = 2;
} else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data
*sread.p++ = sread.incomingByte;
sread.idx++;
}
// Check if we reached the end of the package
if (sread.idx == sizeof(SerialFeedback)) {
uint16_t checksum;
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
// Check validity of the new data
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
// Copy the new data
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
sread.lastValidDataSerial_time = millis();
2021-03-02 22:48:20 +00:00
} else {
_result=0;
}
sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
}
2021-03-02 22:48:20 +00:00
/*
// Print data to built-in Serial
Serial.print("1: "); Serial.print(Feedback.cmd1);
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
Serial.print(" 3: "); Serial.print(Feedback.speedR);
Serial.print(" 4: "); Serial.print(Feedback.speedL);
Serial.print(" 5: "); Serial.print(Feedback.speedR_meas);
Serial.print(" 6: "); Serial.print(Feedback.speedL_meas);
Serial.print(" 7: "); Serial.print(Feedback.batVoltage);
Serial.print(" 8: "); Serial.println(Feedback.boardTemp);
} else {
Serial.println("Non-valid data skipped");
}*/
// Update previous states
sread.incomingBytePrev = sread.incomingByte;
return _result; //new data was available
}
// ########################## SETUP ##########################
void setup()
{
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
2021-03-02 22:48:20 +00:00
Serial2.begin(SERIAL_CONTROL_BAUD); //control
Serial3.begin(SERIAL_CONTROL_BAUD); //control
2021-03-02 22:48:20 +00:00
pinMode(PIN_THROTTLE, INPUT_PULLUP);
}
2021-03-02 22:48:20 +00:00
unsigned long loopmillis;
// ########################## LOOP ##########################
void loop() {
loopmillis=millis(); //read millis for this cycle
2021-03-02 22:48:20 +00:00
bool newData=false;
newData=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
//Serial.print("fo="); Serial.println(count);
//count++;
2021-03-02 22:48:20 +00:00
if (newData) {
float _current = (FeedbackFront.curL_DC+FeedbackFront.curR_DC)/2.0 / 50;
Serial.print(FeedbackFront.curL_DC); Serial.print(", ");
Serial.print(FeedbackFront.curR_DC); Serial.print(", ");
Serial.print(", mean="); Serial.print(_current);
Serial.print(", ");
Serial.print(FeedbackFront.cmd1); Serial.print(", ");
Serial.print(FeedbackFront.cmd2); Serial.print(", ");
Serial.println();
}
if (loopmillis - last_send > SENDPERIOD) {
last_send=loopmillis;
2021-03-02 22:48:20 +00:00
uint16_t throttle_raw = analogRead(PIN_THROTTLE);
Serial.print("Analog: "); Serial.print(throttle_raw);
out_speedFL=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000)));
2021-03-02 22:48:20 +00:00
Serial.print(", Send: "); Serial.println(out_speedFL);
SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2);
}
}