From 7975ca4c7d13a8c6dd448e2a1bd510ea7fce312d Mon Sep 17 00:00:00 2001 From: Fisch Date: Fri, 6 Aug 2021 16:20:48 +0200 Subject: [PATCH] linearize throttle --- controller_teensy/src/main.cpp | 41 +++++- .../20210806_throttleLinearized.csv | 26 ++++ .../20210806_throttleRaw.csv | 26 ++++ .../averageValueReader/averageValueReader.pde | 139 ++++++++++++++++++ 4 files changed, 229 insertions(+), 3 deletions(-) create mode 100644 other/averageValueReader/20210806_throttleLinearized.csv create mode 100644 other/averageValueReader/20210806_throttleRaw.csv create mode 100644 other/averageValueReader/averageValueReader.pde diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 5a432bd..1a4647e 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -34,10 +34,11 @@ bool controllerRear_connected=false; bool controllers_connected=false; #define PIN_THROTTLE A7 -const uint16_t calib_throttle_min = 420; //better a bit too high than too low -const uint16_t calib_throttle_max = 790; +//const uint16_t calib_throttle_min = 420; //better a bit too high than too low +//const uint16_t calib_throttle_max = 790; const uint16_t failsafe_throttle_min = 20; //if adc value falls below this failsafe is triggered const uint16_t failsafe_throttle_max = 1000; //if adc value goes above this failsafe is triggered +const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel #define PIN_BRAKE A8 const uint16_t calib_brake_min = 100;//better a bit too high than too low const uint16_t calib_brake_max = 600; @@ -186,6 +187,8 @@ void leds(); void readButtons(); +uint16_t linearizeThrottle(uint16_t v); + void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef) { // Create command @@ -420,7 +423,8 @@ void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg) void readADC() { throttle_raw = analogRead(PIN_THROTTLE); - throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain + //maps throttle curve to be linear + throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain brake_raw = analogRead(PIN_BRAKE); brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain //brake_pos = (int16_t)(pow((brake_pos/1000.0),2)*1000); @@ -583,6 +587,12 @@ void sendCMD() { log_update=true; //Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000); Serial.println() + + }else if(loopmillis>last_log_send+LOGMININTERVAL){ + //Serial.print(throttle_raw); Serial.println(); + + Serial.print(linearizeThrottle(throttle_raw)); Serial.println(); + last_log_send=loopmillis; } } @@ -651,4 +661,29 @@ void readButtons() { } } +} + +uint16_t linearizeThrottle(uint16_t v) { + //input is raw adc value from hall sensor + //uses throttleCurvePerMM array to find linear approximation of actual throttle travel + //array has to be sorted ! + uint8_t _searchpos=0; + uint8_t arraysize = sizeof(throttleCurvePerMM)/sizeof(throttleCurvePerMM[0]); + while (_searchpos < arraysize && v>throttleCurvePerMM[_searchpos]) { //find arraypos with value above input value + _searchpos++; //try next value + } + + if (_searchpos <=0) { //lower limit + return 0; + } + if (_searchpos >= arraysize) { //upper limit + return 1000; + } + + uint16_t nextLower=throttleCurvePerMM[_searchpos-1]; + uint16_t nextHigher=throttleCurvePerMM[_searchpos]; + float _linearThrottle = _searchpos+map(v*1.0,nextLower,nextHigher,0.0,1.0); + _linearThrottle/=arraysize; //scale to 0-1 + _linearThrottle*=1000; //scale to 0-1000 + return (uint16_t)_linearThrottle; } \ No newline at end of file diff --git a/other/averageValueReader/20210806_throttleLinearized.csv b/other/averageValueReader/20210806_throttleLinearized.csv new file mode 100644 index 0000000..ad0afe3 --- /dev/null +++ b/other/averageValueReader/20210806_throttleLinearized.csv @@ -0,0 +1,26 @@ +id,value +0,0.0 +1,0.0 +2,52.05 +3,103.69 +4,130.77 +5,179.51 +6,231.12 +7,275.7 +8,317.91 +9,370.68 +10,409.84 +11,454.08 +12,492.48 +13,544.73 +14,582.83 +15,636.04 +16,681.43 +17,728.09 +18,773.84 +19,816.48 +20,862.65 +21,905.05 +22,966.9 +23,1000.0 +24,1000.0 diff --git a/other/averageValueReader/20210806_throttleRaw.csv b/other/averageValueReader/20210806_throttleRaw.csv new file mode 100644 index 0000000..a579b60 --- /dev/null +++ b/other/averageValueReader/20210806_throttleRaw.csv @@ -0,0 +1,26 @@ +id,value,mm_pressed,v_per_mm +0,302.5,0, +1,371.86,1,69.36 +2,414.47,2,42.61 +3,459.96,3,45.49 +4,489.85,4,29.89 +5,511.22,5,21.37 +6,527.16,6,15.9399999999999 +7,538.97,7,11.8100000000001 +8,547.93,8,8.95999999999992 +9,555,9,7.07000000000005 +10,561.18,10,6.17999999999995 +11,566.99,11,5.81000000000006 +12,573.43,12,6.43999999999994 +13,577.98,13,4.55000000000007 +14,583.67,14,5.68999999999994 +15,590.4,15,6.73000000000002 +16,599.08,16,8.68000000000006 +17,611.88,17,12.8 +18,630.02,18,18.14 +19,656.96,19,26.9400000000001 +20,697.33,20,40.37 +21,753.98,21,56.65 +22,789.21,22,35.23 +23,796.87,23,7.65999999999997 +24,796.78,24,-0.090000000000032 diff --git a/other/averageValueReader/averageValueReader.pde b/other/averageValueReader/averageValueReader.pde new file mode 100644 index 0000000..67a5b69 --- /dev/null +++ b/other/averageValueReader/averageValueReader.pde @@ -0,0 +1,139 @@ +import processing.serial.*; + +//String serial_port="COM3"; +String serial_port="/dev/ttyACM0"; +Serial serial; +String serialString=""; //last read string +int serial_endchar=10; //10=ASCII Linefeed + +float lastValue=0; + +int valueNum=100; +float[] valueArray= new float[valueNum]; +int valueArrayPos=0; //next to be written array position + +long restartTime=0; + +boolean running=false; + +float resultAverage=0; +float resultSpan=0; + +color bg=color(0); +color text=color(255); + +Table table; + + +void setup() { + size(400, 200); + frameRate(100); + + printArray(Serial.list()); + // Open the port you are using at the rate you want: + serial = new Serial(this, serial_port, 115200); + + serial.clear(); + // Throw out the first reading, in case we started reading + // in the middle of a string from the sender. + println("readUntil"); + serialString = serial.readStringUntil(serial_endchar); + println("read:"+serialString); + serialString = null; + + table = new Table(); + + + table.addColumn("id"); + table.addColumn("value"); + +} + +void draw() { + if (serial.available() > 0) { + serialString = serial.readStringUntil(serial_endchar); + + if (serialString != null) { + serialString=trim(serialString); + //println(serialString); + String[] list = split(serialString, ','); + lastValue=parseFloat(list[0]); + + if (running) { + processNewValue(lastValue); + if (valueArrayPos>=valueArray.length) { //full + running=false; + resultAverage=getAverage(); + resultSpan=getSpan(); + println("Finished reading "+valueNum+ " values in "+(millis()-restartTime)+" ms"); + println("Average="+resultAverage); + println("Span="+resultSpan); + + } + } + + } + } + + background(bg); + + textAlign(LEFT); + textSize(16); + text(lastValue, 8,16); + + textAlign(CENTER); + textSize(48); + text(resultAverage, width/2,height/3); + + textAlign(CENTER); + textSize(16); + text("Span", width/2,height-48-20); + textSize(48); + text(resultSpan, width/2,height-20); +} + +void keyPressed() { + println("key="+keyCode); + + if (keyCode==10) { //10=enter + println("restart"); + valueArrayPos=0; + restartTime=millis(); + running=true; + }else if(keyCode==83){ //83=s save + print("save value to table. "); + + TableRow newRow = table.addRow(); + newRow.setInt("id", table.getRowCount() - 1); + newRow.setFloat("value", resultAverage); + println("table size="+table.getRowCount()); + + + + }else if(keyCode==87) { //87=w write to file + String tableFilename="new.csv"; + println("write table to file "+tableFilename); + saveTable(table, tableFilename); + } +} + +void processNewValue(float v) { + if (valueArrayPos