From b30e350cd1d1c0ec08e161645a55ba48a128cefd Mon Sep 17 00:00:00 2001 From: Fisch Date: Fri, 20 Dec 2019 22:21:21 +0100 Subject: [PATCH] add current feedback --- .../controller_stm_PotiTest_NewFoc.ino | 16 +++++++++++++--- Src/main.c | 9 ++++++++- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/02_Arduino/controller_stm_PotiTest_NewFoc/controller_stm_PotiTest_NewFoc.ino b/02_Arduino/controller_stm_PotiTest_NewFoc/controller_stm_PotiTest_NewFoc.ino index ae269bf..2744fad 100644 --- a/02_Arduino/controller_stm_PotiTest_NewFoc/controller_stm_PotiTest_NewFoc.ino +++ b/02_Arduino/controller_stm_PotiTest_NewFoc/controller_stm_PotiTest_NewFoc.ino @@ -46,6 +46,8 @@ long last_send = 0; +long last_validReceive=0; + // Global variables uint8_t idx = 0; // Index for new data pointer @@ -72,6 +74,8 @@ typedef struct{ int16_t speedL_meas; int16_t batVoltage; int16_t boardTemp; + int16_t curL_DC; + int16_t curR_DC; int16_t checksum; } SerialFeedback; SerialFeedback Feedback; @@ -138,13 +142,15 @@ void ReceiveSerial1() if (idx == 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); + ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC); // Check validity of the new data if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { // Copy the new data memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); + Serial.print(millis()-last_validReceive); Serial.print("ms "); + // Print data to built-in Serial Serial.print("1: "); Serial.print(Feedback.cmd1); Serial.print(" 2: "); Serial.print(Feedback.cmd2); @@ -153,7 +159,11 @@ void ReceiveSerial1() 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); + Serial.print(" 8: "); Serial.print(Feedback.boardTemp); + Serial.print(" 9: "); Serial.print(Feedback.curL_DC); + Serial.print(" 10: "); Serial.println(Feedback.curR_DC); + + last_validReceive=millis(); } else { Serial.println("Non-valid data skipped"); } @@ -175,7 +185,7 @@ void loop() { if (millis() - last_send > SENDPERIOD) { - int16_t speedvalue=constrain(analogRead(PIN_POTI)*1.0/MAXADCVALUE*1500, 0, 1500); + int16_t speedvalue=constrain(analogRead(PIN_POTI)*1.0/MAXADCVALUE*1000, 0, 1000); SendSerial1(speedvalue,speedvalue); Serial.print("millis="); Serial.print(millis()); Serial.print(", steer=0"); Serial.print(", speed="); Serial.println(speedvalue); diff --git a/Src/main.c b/Src/main.c index 2855ea5..b714bf2 100644 --- a/Src/main.c +++ b/Src/main.c @@ -91,6 +91,8 @@ typedef struct{ int16_t speedL_meas; int16_t batVoltage; int16_t boardTemp; + int16_t curL_DC; + int16_t curR_DC; uint16_t checksum; } SerialFeedback; static SerialFeedback Feedback; @@ -129,6 +131,9 @@ extern uint8_t enable; // global variable for motor enable extern volatile uint32_t timeout; // global variable for timeout extern int16_t batVoltage; // global variable for battery voltage +extern int16_t curL_DC; // global variable for left motor current. to get current in Ampere divide by A2BIT_CONV +extern int16_t curR_DC; // global variable for right motor current + static uint32_t inactivity_timeout_counter; extern uint8_t nunchuck_data[6]; @@ -463,8 +468,10 @@ int main(void) { Feedback.speedL_meas = (int16_t)rtY_Right.n_mot; Feedback.batVoltage = (int16_t)(batVoltage * BAT_CALIB_REAL_VOLTAGE / BAT_CALIB_ADC); Feedback.boardTemp = (int16_t)board_temp_deg_c; + Feedback.curL_DC = (int16_t)curL_DC*10/A2BIT_CONV*100; //in milli Amperes, 100mA resolution + Feedback.curR_DC = (int16_t)curR_DC*10/A2BIT_CONV*100; Feedback.checksum = (uint16_t)(Feedback.start ^ Feedback.cmd1 ^ Feedback.cmd2 ^ Feedback.speedR ^ Feedback.speedL - ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp); + ^ Feedback.speedR_meas ^ Feedback.speedL_meas ^ Feedback.batVoltage ^ Feedback.boardTemp ^ Feedback.curL_DC ^Feedback.curR_DC); UART_DMA_CHANNEL->CCR &= ~DMA_CCR_EN; UART_DMA_CHANNEL->CNDTR = sizeof(Feedback);