From 53ee2f4d224b679919107be41967d7bd9644dcc0 Mon Sep 17 00:00:00 2001 From: Fisch Date: Wed, 11 Dec 2019 01:16:24 +0100 Subject: [PATCH] working basic functionality for testing --- controller/controller.ino | 205 +++++++++++++++++++++++++++++--------- 1 file changed, 158 insertions(+), 47 deletions(-) diff --git a/controller/controller.ino b/controller/controller.ino index 4b399a2..85fcab7 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -40,14 +40,17 @@ //#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_POWERBUTTON PB8 //"Enginge Start" Button. connected To NC (=LOW). HIGH when pressed #define POWERBUTTON_DOWN digitalRead(PIN_POWERBUTTON) -#define SENDPERIOD 200 //ms. delay for sending speed and steer data to motor controller via serial -#define PIN_THROTTLE PA0 +#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial +#define PIN_THROTTLE PA4 +#define PIN_BRAKE PA5 #define PIN_ENABLE PB9 @@ -65,11 +68,19 @@ long last_send = 0; // Global variables -uint8_t idx = 0; // Index for new data pointer -uint16_t bufStartFrame; // Buffer Start Frame -byte *p; // Pointer declaration for the new received data -byte incomingByte; -byte incomingBytePrev; +uint8_t idx1 = 0; // Index for new data pointer +uint16_t bufStartFrame1; // Buffer Start Frame +byte *p1; // Pointer declaration for the new received data +byte incomingByte1; +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{ uint16_t start; @@ -77,7 +88,8 @@ typedef struct{ int16_t speedRight; uint16_t checksum; } SerialCommand; -SerialCommand Command; +SerialCommand Command1; +SerialCommand Command2; typedef struct{ uint16_t start; @@ -91,8 +103,10 @@ typedef struct{ int16_t boardTemp; int16_t checksum; } SerialFeedback; -SerialFeedback Feedback; -SerialFeedback NewFeedback; +SerialFeedback Feedback1; +SerialFeedback NewFeedback1; +SerialFeedback Feedback2; +SerialFeedback NewFeedback2; // ########################## SETUP ########################## void setup() @@ -100,8 +114,8 @@ void setup() Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) - Serial1.begin(19200); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2) - Serial2.begin(19200); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) + Serial1.begin(38400); //control. A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector + Serial2.begin(38400); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector Serial1.begin(SERIAL_CONTROL_BAUD); @@ -114,6 +128,8 @@ void setup() pinMode(PIN_MODELED_RED, OUTPUT); pinMode(PIN_RELAISFRONT, OUTPUT); pinMode(PIN_RELAISREAR, OUTPUT); + pinMode(PIN_THROTTLE, INPUT); + pinMode(PIN_BRAKE, INPUT); Serial.println("Initialized"); } @@ -122,22 +138,29 @@ void setup() void SendSerial1(int16_t uSpeedLeft, int16_t uSpeedRight) { // Create command - Command.start = (uint16_t)START_FRAME; - Command.speedLeft = (int16_t)uSpeedLeft; - Command.speedRight = (int16_t)uSpeedRight; - Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight); - - // Write to Serial - Serial1.write((uint8_t *) &Command, sizeof(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); + Serial1.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); + Serial2.write((uint8_t *) &Command2, sizeof(Command2)); } // ########################## RECEIVE ########################## void ReceiveSerial1() { // Check for new data availability in the Serial buffer - if (Serial1.available()) { - incomingByte = Serial1.read(); // Read the incoming byte - bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame + if ( Serial1.available() ) { + incomingByte1 = Serial1.read(); // Read the incoming byte + bufStartFrame1 = ((uint16_t)(incomingBytePrev1) << 8) + incomingByte1; // Construct the start frame } else { return; @@ -145,32 +168,34 @@ void ReceiveSerial1() // If DEBUG_RX is defined print all incoming bytes #ifdef DEBUG_RX - Serial.print(incomingByte); + Serial.print(incomingByte1); return; #endif // Copy received data - if (bufStartFrame == START_FRAME) { // Initialize if new data is detected - p = (byte *)&NewFeedback; - *p++ = incomingBytePrev; - *p++ = incomingByte; - idx = 2; - } else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data - *p++ = incomingByte; - idx++; + 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 (idx == sizeof(SerialFeedback)) { + if (idx1 == sizeof(SerialFeedback)) { uint16_t checksum; - checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL - ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp); + checksum = (uint16_t)(NewFeedback1.start ^ NewFeedback1.cmd1 ^ NewFeedback1.cmd2 ^ NewFeedback1.speedR ^ NewFeedback1.speedL + ^ NewFeedback1.speedR_meas ^ NewFeedback1.speedL_meas ^ NewFeedback1.batVoltage ^ NewFeedback1.boardTemp); // 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 - 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 Serial.print("1: "); Serial.print(Feedback.cmd1); Serial.print(" 2: "); Serial.print(Feedback.cmd2); @@ -182,12 +207,58 @@ void ReceiveSerial1() Serial.print(" 8: "); Serial.println(Feedback.boardTemp); } else { 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 - 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 ########################## @@ -195,7 +266,7 @@ void ReceiveSerial1() void loop() { - selfTest(); //start selftest, does not return + //selfTest(); //start selftest, does not return ReceiveSerial1(); // Check for new received data @@ -204,13 +275,24 @@ void loop() { } 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); - //Serial.print("millis="); Serial.print(millis()); Serial.print(", steer=0"); Serial.print(", speed="); Serial.println(speedvalue); + if (MODESWITCH_DOWN) { + 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(); @@ -223,9 +305,38 @@ void loop() { } 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(); }