bobbycar/controller_teensy/include/comms.h

162 lines
6.6 KiB
C

#ifndef _COMMS_H_
#define _COMMS_H_
#include "definitions.h"
#include "structs.h"
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis);
void writeLogInfo(HardwareSerial &SerialRef);
void writeLogHeader(HardwareSerial &SerialRef);
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake);
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg);
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);
SerialRef.write((uint8_t *) &scom, sizeof(scom));
}
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
{
bool _result=false; //return true if new full data frame received
// Check for new data availability in the Serial buffer
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 false; //nothing new
}
// If DEBUG_RX is defined print all incoming bytes
#ifdef DEBUG_RX
Serial.print(sread.incomingByte);
#endif
// Copy received data
if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected
sread.p = (byte *)&NewFeedback;
*sread.p++ = sread.incomingBytePrev;
*sread.p++ = sread.incomingByte;
sread.idx = 2;
} else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data
*sread.p++ = sread.incomingByte;
sread.idx++;
}
// Check if we reached the end of the package
if (sread.idx == sizeof(SerialFeedback)) {
uint16_t checksum;
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
// Check validity of the new data
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
// Copy the new data
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
sread.lastValidDataSerial_time = millis();
_result=true;
} else {
_result=false;
}
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;
return _result; //new data was available
}
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis) {
mp.cur_pos++;
mp.cur_pos%=CURRENT_FILTER_SIZE;
mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
mp.millis=_loopmillis;
log_update=true;
}
/*
void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
SerialRef.print("#TIMESTAMP:");
SerialRef.println(now());
}
void writeLogHeader(HardwareSerial &SerialRef) {
SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
SerialRef.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed");
}
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
{
SerialRef.print(time/1000.0,3); SerialRef.print(","); //time in seconds
SerialRef.print(mpfront.cmdL); SerialRef.print(",");
SerialRef.print(mpfront.cmdR); SerialRef.print(",");
SerialRef.print(mprear.cmdL); SerialRef.print(",");
SerialRef.print(mprear.cmdR); SerialRef.print(",");
SerialRef.print(mpfront.filtered_curL,3); SerialRef.print(",");
SerialRef.print(mpfront.filtered_curR,3); SerialRef.print(",");
SerialRef.print(mprear.filtered_curL,3); SerialRef.print(",");
SerialRef.print(mprear.filtered_curR,3); SerialRef.print(",");
SerialRef.print(fbfront.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
SerialRef.print(-fbfront.speedR_meas); SerialRef.print(",");
SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
SerialRef.print(-fbrear.speedR_meas); SerialRef.print(",");
SerialRef.print(fbfront.boardTemp/10.0,1); SerialRef.print(","); //in degC
SerialRef.print(fbrear.boardTemp/10.0,1); SerialRef.print(","); //in degC
SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(currentAll,3); SerialRef.print(",");
SerialRef.print(throttle); SerialRef.print(",");
SerialRef.print(brake); SerialRef.print(",");
SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
SerialRef.print(trip); SerialRef.print(","); //in m
SerialRef.print(currentConsumed,3); SerialRef.println(); //in Ah (Amphours)
}
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
{
SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
}
*/
#endif