linearize throttle

master
interfisch 1 year ago
parent f252d595b0
commit 7975ca4c7d
  1. 41
      controller_teensy/src/main.cpp
  2. 26
      other/averageValueReader/20210806_throttleLinearized.csv
  3. 26
      other/averageValueReader/20210806_throttleRaw.csv
  4. 139
      other/averageValueReader/averageValueReader.pde

@ -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;
}

@ -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
unable to load file from base commit

@ -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
unable to load file from base commit

@ -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);
}
Loading…
Cancel
Save