131 lines
4.8 KiB
C++
131 lines
4.8 KiB
C++
|
|
// ########################## SEND ##########################
|
|
void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight)
|
|
{
|
|
// Create command
|
|
Command1.start = (uint16_t)START_FRAME;
|
|
Command1.speedLeft = (int16_t)uSpeedLeft;
|
|
Command1.speedRight = (int16_t)uSpeedRight;
|
|
Command1.checksum = (uint16_t)(Command1.start ^ Command1.speedLeft ^ Command1.speedRight);
|
|
Serial2.write((uint8_t *) &Command1, sizeof(Command1));
|
|
}
|
|
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);
|
|
Serial3.write((uint8_t *) &Command2, sizeof(Command2));
|
|
}
|
|
|
|
// ########################## RECEIVE ##########################
|
|
void ReceiveSerial1()
|
|
{
|
|
// Check for new data availability in the Serial buffer
|
|
if ( Serial2.available() ) {
|
|
incomingByte1 = Serial2.read(); // Read the incoming byte
|
|
bufStartFrame1 = ((uint16_t)(incomingBytePrev1) << 8) + incomingByte1; // Construct the start frame
|
|
}
|
|
else {
|
|
return;
|
|
}
|
|
|
|
// If DEBUG_RX is defined print all incoming bytes
|
|
#ifdef DEBUG_RX
|
|
Serial.print(incomingByte1);
|
|
return;
|
|
#endif
|
|
|
|
// Copy received data
|
|
if (bufStartFrame1 == START_FRAME) { // Initialize if new data is detected
|
|
p1 = (byte *)&NewFeedback1;
|
|
*p1++ = incomingBytePrev1;
|
|
*p1++ = incomingByte1;
|
|
idx1 = 2;
|
|
} else if (idx1 >= 2 && idx1 < sizeof(SerialFeedback)) { // Save the new received data
|
|
*p1++ = incomingByte1;
|
|
idx1++;
|
|
}
|
|
|
|
// Check if we reached the end of the package
|
|
if (idx1 == sizeof(SerialFeedback)) {
|
|
uint16_t checksum;
|
|
|
|
checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL
|
|
^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp ^ NewFeedback1.curL_DC ^ NewFeedback1.curR_DC);
|
|
|
|
// Check validity of the new data
|
|
if (NewFeedback1.start == START_FRAME && checksum == NewFeedback1.checksum) {
|
|
// Copy the new data
|
|
memcpy(&Feedback1, &NewFeedback1, sizeof(SerialFeedback));
|
|
lastValidDataSerial1_time = millis();
|
|
}
|
|
idx1 = 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
|
|
incomingBytePrev1 = incomingByte1;
|
|
}
|
|
void ReceiveSerial2()
|
|
{
|
|
// Check for new data availability in the Serial buffer
|
|
if ( Serial3.available() ) {
|
|
incomingByte2 = Serial3.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 ^ NewFeedback2.curL_DC ^ NewFeedback2.curR_DC);
|
|
|
|
// Check validity of the new data
|
|
if (NewFeedback2.start == START_FRAME && checksum == NewFeedback2.checksum) {
|
|
// Copy the new data
|
|
memcpy(&Feedback2, &NewFeedback2, sizeof(SerialFeedback));
|
|
lastValidDataSerial2_time = millis();
|
|
}
|
|
idx2 = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
|
|
|
}
|
|
|
|
// Update previous states
|
|
incomingBytePrev2 = incomingByte2;
|
|
}
|