fix serial esc test control

This commit is contained in:
interfisch 2021-03-02 23:48:20 +01:00
parent e62286cb8b
commit 649daaa25e
3 changed files with 79 additions and 45 deletions

View File

@ -13,3 +13,9 @@ platform = teensy
board = teensy31 board = teensy31
framework = arduino framework = arduino
monitor_speed = 115200
build_flags =
-D USB_SERIAL_HID

View File

@ -1,21 +1,25 @@
#include <Arduino.h> #include <Arduino.h>
// ########################## DEFINES ########################## // ########################## DEFINES ##########################
#define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) #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) #define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication #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 #define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial
#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_speedFL=0;
int16_t out_speedFR=0; int16_t out_speedFR=0;
int16_t out_speedRL=0; int16_t out_speedRL=0;
int16_t out_speedRR=0; int16_t out_speedRR=0;
unsigned long last_send = 0; unsigned long last_send = 0;
unsigned long last_receive = 0;
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight);
void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback);
// Global variables for serial communication // Global variables for serial communication
typedef struct{ typedef struct{
@ -25,8 +29,6 @@ typedef struct{
byte incomingByte; byte incomingByte;
byte incomingBytePrev; byte incomingBytePrev;
long lastValidDataSerial_time; long lastValidDataSerial_time;
long boardlastPoweron=0; //mainly for failcheck
long boardlastPoweroff=0;
} SerialRead; } SerialRead;
SerialRead SerialcomFront; SerialRead SerialcomFront;
@ -55,31 +57,37 @@ typedef struct{
SerialFeedback FeedbackFront; SerialFeedback FeedbackFront;
SerialFeedback NewFeedbackFront; SerialFeedback NewFeedbackFront;
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight)
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 // Create command
scom.start = (uint16_t)START_FRAME; scom.start = (uint16_t)START_FRAME;
scom.speedLeft = (int16_t)uSpeedLeft; scom.speedLeft = (int16_t)uSpeedLeft;
scom.speedRight = (int16_t)uSpeedRight; scom.speedRight = (int16_t)uSpeedRight;
scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight); scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
Serial1.write((uint8_t *) &scom, sizeof(scom)); //TODO: use correct serial object
SerialRef.write((uint8_t *) &scom, sizeof(scom));
} }
void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback) bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
{ {
bool _result=1;
// Check for new data availability in the Serial buffer // Check for new data availability in the Serial buffer
if ( Serial1.available() ) { if ( SerialRef.available() ) {
sread.incomingByte = Serial1.read(); // Read the incoming byte sread.incomingByte = SerialRef.read(); // Read the incoming byte
sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame
} }
else { else {
return; return 0;
} }
// If DEBUG_RX is defined print all incoming bytes // If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX #ifdef DEBUG_RX
Serial.print(sread.incomingByte); Serial.print(sread.incomingByte);
return;
#endif #endif
// Copy received data // Copy received data
@ -105,26 +113,31 @@ void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
// Copy the new data // Copy the new data
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
sread.lastValidDataSerial_time = millis(); sread.lastValidDataSerial_time = millis();
} else {
_result=0;
} }
sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
/*
// 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;
} }
/*
// 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 ########################## // ########################## SETUP ##########################
void setup() void setup()
@ -132,33 +145,48 @@ void setup()
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial1.begin(SERIAL_CONTROL_BAUD); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear) Serial2.begin(SERIAL_CONTROL_BAUD); //control
Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front) Serial3.begin(SERIAL_CONTROL_BAUD); //control
pinMode(PIN_THROTTLE, INPUT_PULLUP);
} }
unsigned long loopmillis;
// ########################## LOOP ########################## // ########################## LOOP ##########################
void loop() { void loop() {
loopmillis=millis(); //read millis for this cycle loopmillis=millis(); //read millis for this cycle
ReceiveSerial(); bool newData=false;
newData=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
//Serial.print("fo="); Serial.println(count);
//count++;
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) { if (loopmillis - last_send > SENDPERIOD) {
last_send=loopmillis; last_send=loopmillis;
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)));
SendSerial(out_speedRL,out_speedRR); Serial.print(", Send: "); Serial.println(out_speedFL);
SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2);
float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50;
Serial.print(Feedback1.curL_DC); Serial.print(", "); //1 is rear
Serial.print(Feedback1.curR_DC); Serial.print(", ");
Serial.print(Feedback2.curL_DC); Serial.print(", "); //2 is front
Serial.print(Feedback2.curR_DC);
Serial.print(", mean="); Serial.print(_current);
Serial.println();
} }
} }

@ -1 +1 @@
Subproject commit 2bab3aa1a46d1754ecc81692524120122b5b07f7 Subproject commit 898898a63c6b494e0525df8a8ed2da98fcd5cdb1