From 3becfe30b143343339e9051b47c4569a36e61c00 Mon Sep 17 00:00:00 2001 From: Fisch Date: Tue, 9 Mar 2021 19:45:36 +0100 Subject: [PATCH] test current graph output --- controller_teensy/src/main.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index fe4da04..5ec6c4a 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -20,6 +20,9 @@ int16_t out_speedRR=0; unsigned long last_send = 0; unsigned long last_receive = 0; +float avg_currentL=0; +float avg_currentR=0; + // Global variables for serial communication typedef struct{ @@ -168,6 +171,7 @@ void loop() { if (newData) { float _current = (FeedbackFront.curL_DC+FeedbackFront.curR_DC)/2.0 / 50; + /* Serial.print(FeedbackFront.curL_DC); Serial.print(", "); Serial.print(FeedbackFront.curR_DC); Serial.print(", "); @@ -176,17 +180,24 @@ void loop() { Serial.print(FeedbackFront.cmd1); Serial.print(", "); Serial.print(FeedbackFront.cmd2); Serial.print(", "); Serial.println(); + */ + + #define CURRENTFILTER 0.95 + avg_currentL=avg_currentL*CURRENTFILTER+FeedbackFront.curL_DC*(1-CURRENTFILTER); + avg_currentR=avg_currentR*CURRENTFILTER+FeedbackFront.curR_DC*(1-CURRENTFILTER); } if (loopmillis - last_send > SENDPERIOD) { last_send=loopmillis; uint16_t throttle_raw = analogRead(PIN_THROTTLE); - Serial.print("Analog: "); Serial.print(throttle_raw); - out_speedFL=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); + //Serial.print("Analog: "); Serial.print(throttle_raw); + out_speedFR=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); - Serial.print(", Send: "); Serial.println(out_speedFL); + //Serial.print(", Send: "); Serial.println(out_speedFL); SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2); + + Serial.print(out_speedFL); Serial.print(", "); Serial.print(out_speedFR); Serial.print(", "); Serial.print(avg_currentR); Serial.print(", "); Serial.println(avg_currentL); } } \ No newline at end of file