bobbycar/controller/serialControl.ino

131 lines
4.8 KiB
Arduino
Raw Normal View History

2019-12-11 20:22:54 +00:00
// ########################## 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));
2019-12-11 20:22:54 +00:00
}
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));
2019-12-11 20:22:54 +00:00
}
// ########################## RECEIVE ##########################
void ReceiveSerial1()
{
// Check for new data availability in the Serial buffer
if ( Serial2.available() ) {
incomingByte1 = Serial2.read(); // Read the incoming byte
2019-12-11 20:22:54 +00:00
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;
2019-12-28 23:41:39 +00:00
2019-12-11 20:22:54 +00:00
checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL
2019-12-28 23:41:39 +00:00
^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp ^ NewFeedback1.curL_DC ^ NewFeedback1.curR_DC);
2019-12-11 20:22:54 +00:00
// 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
2019-12-11 20:22:54 +00:00
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
2019-12-28 23:41:39 +00:00
^ NewFeedback2.speedR_meas ^ NewFeedback2.speedL_meas ^ NewFeedback2.batVoltage ^ NewFeedback2.boardTemp ^ NewFeedback2.curL_DC ^ NewFeedback2.curR_DC);
2019-12-11 20:22:54 +00:00
// 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;
}