working basic functionality for testing
This commit is contained in:
parent
433dfbe73b
commit
53ee2f4d22
|
@ -40,14 +40,17 @@
|
||||||
|
|
||||||
//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
|
//#define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable)
|
||||||
|
|
||||||
#define MAXADCVALUE 4095
|
//#define MAXADCVALUE 4095
|
||||||
|
#define ADC_CALIB_THROTTLE_MIN 2000
|
||||||
|
#define ADC_CALIB_THROTTLE_MAX 3120
|
||||||
|
|
||||||
#define PIN_POWERLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor
|
#define PIN_POWERLED PA0 //Red LED inside Engine Start Button. Powered with 5V via transistor
|
||||||
#define PIN_POWERBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed
|
#define PIN_POWERBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed
|
||||||
#define POWERBUTTON_DOWN digitalRead(PIN_POWERBUTTON)
|
#define POWERBUTTON_DOWN digitalRead(PIN_POWERBUTTON)
|
||||||
|
|
||||||
#define SENDPERIOD 200 //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 PIN_THROTTLE PA0
|
#define PIN_THROTTLE PA4
|
||||||
|
#define PIN_BRAKE PA5
|
||||||
|
|
||||||
#define PIN_ENABLE PB9
|
#define PIN_ENABLE PB9
|
||||||
|
|
||||||
|
@ -65,11 +68,19 @@ long last_send = 0;
|
||||||
|
|
||||||
|
|
||||||
// Global variables
|
// Global variables
|
||||||
uint8_t idx = 0; // Index for new data pointer
|
uint8_t idx1 = 0; // Index for new data pointer
|
||||||
uint16_t bufStartFrame; // Buffer Start Frame
|
uint16_t bufStartFrame1; // Buffer Start Frame
|
||||||
byte *p; // Pointer declaration for the new received data
|
byte *p1; // Pointer declaration for the new received data
|
||||||
byte incomingByte;
|
byte incomingByte1;
|
||||||
byte incomingBytePrev;
|
byte incomingBytePrev1;
|
||||||
|
|
||||||
|
//Same for Serial2
|
||||||
|
uint8_t idx2 = 0; // Index for new data pointer
|
||||||
|
uint16_t bufStartFrame2; // Buffer Start Frame
|
||||||
|
byte *p2; // Pointer declaration for the new received data
|
||||||
|
byte incomingByte2;
|
||||||
|
byte incomingBytePrev2;
|
||||||
|
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
uint16_t start;
|
uint16_t start;
|
||||||
|
@ -77,7 +88,8 @@ typedef struct{
|
||||||
int16_t speedRight;
|
int16_t speedRight;
|
||||||
uint16_t checksum;
|
uint16_t checksum;
|
||||||
} SerialCommand;
|
} SerialCommand;
|
||||||
SerialCommand Command;
|
SerialCommand Command1;
|
||||||
|
SerialCommand Command2;
|
||||||
|
|
||||||
typedef struct{
|
typedef struct{
|
||||||
uint16_t start;
|
uint16_t start;
|
||||||
|
@ -91,8 +103,10 @@ typedef struct{
|
||||||
int16_t boardTemp;
|
int16_t boardTemp;
|
||||||
int16_t checksum;
|
int16_t checksum;
|
||||||
} SerialFeedback;
|
} SerialFeedback;
|
||||||
SerialFeedback Feedback;
|
SerialFeedback Feedback1;
|
||||||
SerialFeedback NewFeedback;
|
SerialFeedback NewFeedback1;
|
||||||
|
SerialFeedback Feedback2;
|
||||||
|
SerialFeedback NewFeedback2;
|
||||||
|
|
||||||
// ########################## SETUP ##########################
|
// ########################## SETUP ##########################
|
||||||
void setup()
|
void setup()
|
||||||
|
@ -100,8 +114,8 @@ void setup()
|
||||||
|
|
||||||
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
||||||
|
|
||||||
Serial1.begin(19200); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2)
|
Serial1.begin(38400); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector
|
||||||
Serial2.begin(19200); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3)
|
Serial2.begin(38400); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector
|
||||||
|
|
||||||
|
|
||||||
Serial1.begin(SERIAL_CONTROL_BAUD);
|
Serial1.begin(SERIAL_CONTROL_BAUD);
|
||||||
|
@ -114,6 +128,8 @@ void setup()
|
||||||
pinMode(PIN_MODELED_RED, OUTPUT);
|
pinMode(PIN_MODELED_RED, OUTPUT);
|
||||||
pinMode(PIN_RELAISFRONT, OUTPUT);
|
pinMode(PIN_RELAISFRONT, OUTPUT);
|
||||||
pinMode(PIN_RELAISREAR, OUTPUT);
|
pinMode(PIN_RELAISREAR, OUTPUT);
|
||||||
|
pinMode(PIN_THROTTLE, INPUT);
|
||||||
|
pinMode(PIN_BRAKE, INPUT);
|
||||||
|
|
||||||
Serial.println("Initialized");
|
Serial.println("Initialized");
|
||||||
}
|
}
|
||||||
|
@ -122,22 +138,29 @@ void setup()
|
||||||
void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight)
|
void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight)
|
||||||
{
|
{
|
||||||
// Create command
|
// Create command
|
||||||
Command.start = (uint16_t)START_FRAME;
|
Command1.start = (uint16_t)START_FRAME;
|
||||||
Command.speedLeft = (int16_t)uSpeedLeft;
|
Command1.speedLeft = (int16_t)uSpeedLeft;
|
||||||
Command.speedRight = (int16_t)uSpeedRight;
|
Command1.speedRight = (int16_t)uSpeedRight;
|
||||||
Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight);
|
Command1.checksum = (uint16_t)(Command1.start ^ Command1.speedLeft ^ Command1.speedRight);
|
||||||
|
Serial1.write((uint8_t *) &Command1, sizeof(Command1));
|
||||||
// Write to Serial
|
}
|
||||||
Serial1.write((uint8_t *) &Command, sizeof(Command));
|
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
|
||||||
|
{
|
||||||
|
// Create command
|
||||||
|
Command2.start = (uint16_t)START_FRAME;
|
||||||
|
Command2.speedLeft = (int16_t)uSpeedLeft;
|
||||||
|
Command2.speedRight = (int16_t)uSpeedRight;
|
||||||
|
Command2.checksum = (uint16_t)(Command2.start ^ Command2.speedLeft ^ Command2.speedRight);
|
||||||
|
Serial2.write((uint8_t *) &Command2, sizeof(Command2));
|
||||||
}
|
}
|
||||||
|
|
||||||
// ########################## RECEIVE ##########################
|
// ########################## RECEIVE ##########################
|
||||||
void ReceiveSerial1()
|
void ReceiveSerial1()
|
||||||
{
|
{
|
||||||
// Check for new data availability in the Serial buffer
|
// Check for new data availability in the Serial buffer
|
||||||
if (Serial1.available()) {
|
if ( Serial1.available() ) {
|
||||||
incomingByte = Serial1.read(); // Read the incoming byte
|
incomingByte1 = Serial1.read(); // Read the incoming byte
|
||||||
bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame
|
bufStartFrame1 = ((uint16_t)(incomingBytePrev1) << 8) + incomingByte1; // Construct the start frame
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
return;
|
return;
|
||||||
|
@ -145,32 +168,34 @@ void ReceiveSerial1()
|
||||||
|
|
||||||
// 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(incomingByte);
|
Serial.print(incomingByte1);
|
||||||
return;
|
return;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Copy received data
|
// Copy received data
|
||||||
if (bufStartFrame == START_FRAME) { // Initialize if new data is detected
|
if (bufStartFrame1 == START_FRAME) { // Initialize if new data is detected
|
||||||
p = (byte *)&NewFeedback;
|
p1 = (byte *)&NewFeedback1;
|
||||||
*p++ = incomingBytePrev;
|
*p1++ = incomingBytePrev1;
|
||||||
*p++ = incomingByte;
|
*p1++ = incomingByte1;
|
||||||
idx = 2;
|
idx1 = 2;
|
||||||
} else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data
|
} else if (idx1 >= 2 && idx1 < sizeof(SerialFeedback)) { // Save the new received data
|
||||||
*p++ = incomingByte;
|
*p1++ = incomingByte1;
|
||||||
idx++;
|
idx1++;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if we reached the end of the package
|
// Check if we reached the end of the package
|
||||||
if (idx == sizeof(SerialFeedback)) {
|
if (idx1 == sizeof(SerialFeedback)) {
|
||||||
uint16_t checksum;
|
uint16_t checksum;
|
||||||
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL
|
checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL
|
||||||
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp);
|
^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp);
|
||||||
|
|
||||||
// Check validity of the new data
|
// Check validity of the new data
|
||||||
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
|
if (NewFeedback1.start == START_FRAME && checksum == NewFeedback1.checksum) {
|
||||||
// Copy the new data
|
// Copy the new data
|
||||||
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
memcpy(&Feedback1, &NewFeedback1, sizeof(SerialFeedback));
|
||||||
|
}
|
||||||
|
idx1 = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
||||||
|
/*
|
||||||
// Print data to built-in Serial
|
// Print data to built-in Serial
|
||||||
Serial.print("1: "); Serial.print(Feedback.cmd1);
|
Serial.print("1: "); Serial.print(Feedback.cmd1);
|
||||||
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
|
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
|
||||||
|
@ -182,12 +207,58 @@ void ReceiveSerial1()
|
||||||
Serial.print(" 8: "); Serial.println(Feedback.boardTemp);
|
Serial.print(" 8: "); Serial.println(Feedback.boardTemp);
|
||||||
} else {
|
} else {
|
||||||
Serial.println("Non-valid data skipped");
|
Serial.println("Non-valid data skipped");
|
||||||
}
|
}*/
|
||||||
idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update previous states
|
// Update previous states
|
||||||
incomingBytePrev = incomingByte;
|
incomingBytePrev1 = incomingByte1;
|
||||||
|
}
|
||||||
|
void ReceiveSerial2()
|
||||||
|
{
|
||||||
|
// Check for new data availability in the Serial buffer
|
||||||
|
if ( Serial2.available() ) {
|
||||||
|
incomingByte2 = Serial2.read(); // Read the incoming byte
|
||||||
|
bufStartFrame2 = ((uint16_t)(incomingBytePrev2) << 8) + incomingByte2; // Construct the start frame
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If DEBUG_RX is defined print all incoming bytes
|
||||||
|
#ifdef DEBUG_RX
|
||||||
|
Serial.print(incomingByte2);
|
||||||
|
return;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Copy received data
|
||||||
|
if (bufStartFrame2 == START_FRAME) { // Initialize if new data is detected
|
||||||
|
p2 = (byte *)&NewFeedback2;
|
||||||
|
*p2++ = incomingBytePrev2;
|
||||||
|
*p2++ = incomingByte2;
|
||||||
|
idx2 = 2;
|
||||||
|
} else if (idx2 >= 2 && idx2 < sizeof(SerialFeedback)) { // Save the new received data
|
||||||
|
*p2++ = incomingByte2;
|
||||||
|
idx2++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if we reached the end of the package
|
||||||
|
if (idx2 == sizeof(SerialFeedback)) {
|
||||||
|
uint16_t checksum;
|
||||||
|
checksum = (uint16_t)(NewFeedback2.start ^ NewFeedback2.cmd1 ^ NewFeedback2.cmd2 ^ NewFeedback2.speedR ^ NewFeedback2.speedL
|
||||||
|
^ NewFeedback2.speedR_meas ^ NewFeedback2.speedL_meas ^ NewFeedback2.batVoltage ^ NewFeedback2.boardTemp);
|
||||||
|
|
||||||
|
// Check validity of the new data
|
||||||
|
if (NewFeedback2.start == START_FRAME && checksum == NewFeedback2.checksum) {
|
||||||
|
// Copy the new data
|
||||||
|
memcpy(&Feedback2, &NewFeedback2, sizeof(SerialFeedback));
|
||||||
|
}
|
||||||
|
idx2 = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update previous states
|
||||||
|
incomingBytePrev2 = incomingByte2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// ########################## LOOP ##########################
|
// ########################## LOOP ##########################
|
||||||
|
@ -195,7 +266,7 @@ void ReceiveSerial1()
|
||||||
|
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
selfTest(); //start selftest, does not return
|
//selfTest(); //start selftest, does not return
|
||||||
|
|
||||||
ReceiveSerial1(); // Check for new received data
|
ReceiveSerial1(); // Check for new received data
|
||||||
|
|
||||||
|
@ -204,13 +275,24 @@ void loop() {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (millis() - last_send > SENDPERIOD) {
|
if (millis() - last_send > SENDPERIOD) {
|
||||||
Serial.print("powerbutton="); Serial.print(POWERBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
|
//Serial.print("powerbutton="); Serial.print(POWERBUTTON_DOWN); Serial.print(" modeswitch down="); Serial.println(MODESWITCH_DOWN);
|
||||||
|
|
||||||
|
int _read=analogRead(PIN_THROTTLE);
|
||||||
|
|
||||||
int16_t speedvalue=constrain(analogRead(PIN_THROTTLE)*1.0/MAXADCVALUE*1000, 0, 1000);
|
int16_t speedvalue=constrain( map(_read, ADC_CALIB_THROTTLE_MIN, ADC_CALIB_THROTTLE_MAX, 0, 1000 ) ,0, 1000);
|
||||||
|
|
||||||
SendSerial1(speedvalue,0);
|
if (MODESWITCH_DOWN) {
|
||||||
//Serial.print("millis="); Serial.print(millis()); Serial.print(", steer=0"); Serial.print(", speed="); Serial.println(speedvalue);
|
SendSerial1(speedvalue,0);
|
||||||
|
SendSerial2(speedvalue,0);
|
||||||
|
Serial.print("L_");
|
||||||
|
}else{
|
||||||
|
SendSerial1(0,speedvalue);
|
||||||
|
SendSerial2(0,speedvalue);
|
||||||
|
Serial.print("R_");
|
||||||
|
}
|
||||||
|
Serial.print("millis="); Serial.print(millis()); Serial.print(", adcthrottle="); Serial.print(_read);
|
||||||
|
Serial.print(", 1.L="); Serial.print(Command1.speedLeft); Serial.print(", 1.R="); Serial.print(Command1.speedRight);
|
||||||
|
Serial.print(", 2.L="); Serial.print(Command2.speedLeft); Serial.print(", 2.R="); Serial.println(Command2.speedRight);
|
||||||
|
|
||||||
last_send = millis();
|
last_send = millis();
|
||||||
|
|
||||||
|
@ -223,9 +305,38 @@ void loop() {
|
||||||
}
|
}
|
||||||
|
|
||||||
testcounter++;
|
testcounter++;
|
||||||
|
|
||||||
|
|
||||||
|
//Print Motor values
|
||||||
|
Serial.print("cmd1");
|
||||||
|
Serial.print(", "); Serial.print("cmd2");
|
||||||
|
Serial.print(","); Serial.print("speedR");
|
||||||
|
Serial.print(","); Serial.print("speedL");
|
||||||
|
Serial.print(", "); Serial.print("speedR_meas");
|
||||||
|
Serial.print(","); Serial.print("speedL_meas");
|
||||||
|
Serial.print(", "); Serial.print("batVoltage");
|
||||||
|
Serial.print(", "); Serial.println("boardTemp");
|
||||||
|
Serial.println();
|
||||||
|
Serial.print("1: "); Serial.print(Feedback1.cmd1);
|
||||||
|
Serial.print(", "); Serial.print(Feedback1.cmd2);
|
||||||
|
Serial.print(","); Serial.print(Feedback1.speedR);
|
||||||
|
Serial.print(","); Serial.print(Feedback1.speedL);
|
||||||
|
Serial.print(", "); Serial.print(Feedback1.speedR_meas);
|
||||||
|
Serial.print(","); Serial.print(Feedback1.speedL_meas);
|
||||||
|
Serial.print(", "); Serial.print(Feedback1.batVoltage);
|
||||||
|
Serial.print(", "); Serial.println(Feedback1.boardTemp);
|
||||||
|
Serial.println();
|
||||||
|
Serial.print("2: "); Serial.print(Feedback2.cmd1);
|
||||||
|
Serial.print(", "); Serial.print(Feedback2.cmd2);
|
||||||
|
Serial.print(","); Serial.print(Feedback2.speedR);
|
||||||
|
Serial.print(","); Serial.print(Feedback2.speedL);
|
||||||
|
Serial.print(", "); Serial.print(Feedback2.speedR_meas);
|
||||||
|
Serial.print(","); Serial.print(Feedback2.speedL_meas);
|
||||||
|
Serial.print(", "); Serial.print(Feedback2.batVoltage);
|
||||||
|
Serial.print(", "); Serial.println(Feedback2.boardTemp);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (millis()>60000) {
|
if (millis()>30000 && POWERBUTTON_DOWN) {
|
||||||
poweroff();
|
poweroff();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue