linearize throttle

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;
   }
 }
 
@@ -652,3 +662,28 @@ 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;
+}

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

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

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<valueArray.length){ //not full
+    valueArray[valueArrayPos]=v;
+    valueArrayPos++;
+  }
+}
+
+float getAverage() {
+  float average = 0;
+  for ( int i = 0; i < valueArray.length; ++i )
+  {
+   average += valueArray[i];
+  }
+  average /= (float)(valueArray.length); 
+  return average;
+}
+
+float getSpan() {
+  return max(valueArray)-min(valueArray);
+}