fix serial esc test control
This commit is contained in:
parent
e62286cb8b
commit
649daaa25e
|
@ -13,3 +13,9 @@ platform = teensy
|
|||
board = teensy31
|
||||
framework = arduino
|
||||
|
||||
|
||||
|
||||
monitor_speed = 115200
|
||||
|
||||
build_flags =
|
||||
-D USB_SERIAL_HID
|
|
@ -1,21 +1,25 @@
|
|||
#include <Arduino.h>
|
||||
|
||||
// ########################## 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 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 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;
|
||||
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
|
||||
typedef struct{
|
||||
|
@ -25,8 +29,6 @@ typedef struct{
|
|||
byte incomingByte;
|
||||
byte incomingBytePrev;
|
||||
long lastValidDataSerial_time;
|
||||
long boardlastPoweron=0; //mainly for failcheck
|
||||
long boardlastPoweroff=0;
|
||||
} SerialRead;
|
||||
SerialRead SerialcomFront;
|
||||
|
||||
|
@ -55,31 +57,37 @@ typedef struct{
|
|||
SerialFeedback FeedbackFront;
|
||||
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
|
||||
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);
|
||||
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
|
||||
if ( Serial1.available() ) {
|
||||
sread.incomingByte = Serial1.read(); // Read the incoming byte
|
||||
sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame
|
||||
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 {
|
||||
return;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// If DEBUG_RX is defined print all incoming bytes
|
||||
#ifdef DEBUG_RX
|
||||
Serial.print(sread.incomingByte);
|
||||
return;
|
||||
#endif
|
||||
|
||||
// Copy received data
|
||||
|
@ -105,26 +113,31 @@ void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &N
|
|||
// Copy the new data
|
||||
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
||||
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)
|
||||
/*
|
||||
// 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 ##########################
|
||||
void setup()
|
||||
|
@ -132,33 +145,48 @@ void setup()
|
|||
|
||||
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. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front)
|
||||
Serial2.begin(SERIAL_CONTROL_BAUD); //control
|
||||
Serial3.begin(SERIAL_CONTROL_BAUD); //control
|
||||
|
||||
pinMode(PIN_THROTTLE, INPUT_PULLUP);
|
||||
|
||||
}
|
||||
|
||||
|
||||
unsigned long loopmillis;
|
||||
// ########################## LOOP ##########################
|
||||
void loop() {
|
||||
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) {
|
||||
last_send=loopmillis;
|
||||
|
||||
|
||||
SendSerial(out_speedRL,out_speedRR);
|
||||
|
||||
float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50;
|
||||
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)));
|
||||
|
||||
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();
|
||||
Serial.print(", Send: "); Serial.println(out_speedFL);
|
||||
SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2);
|
||||
|
||||
}
|
||||
}
|
|
@ -1 +1 @@
|
|||
Subproject commit 2bab3aa1a46d1754ecc81692524120122b5b07f7
|
||||
Subproject commit 898898a63c6b494e0525df8a8ed2da98fcd5cdb1
|
Loading…
Reference in New Issue