linearize throttle

This commit is contained in:
interfisch 2021-08-06 16:20:48 +02:00
parent f252d595b0
commit 7975ca4c7d
4 changed files with 229 additions and 3 deletions

View File

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

View File

@ -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
1 id value
2 0 0.0
3 1 0.0
4 2 52.05
5 3 103.69
6 4 130.77
7 5 179.51
8 6 231.12
9 7 275.7
10 8 317.91
11 9 370.68
12 10 409.84
13 11 454.08
14 12 492.48
15 13 544.73
16 14 582.83
17 15 636.04
18 16 681.43
19 17 728.09
20 18 773.84
21 19 816.48
22 20 862.65
23 21 905.05
24 22 966.9
25 23 1000.0
26 24 1000.0

View File

@ -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
1 id value mm_pressed v_per_mm
2 0 302.5 0
3 1 371.86 1 69.36
4 2 414.47 2 42.61
5 3 459.96 3 45.49
6 4 489.85 4 29.89
7 5 511.22 5 21.37
8 6 527.16 6 15.9399999999999
9 7 538.97 7 11.8100000000001
10 8 547.93 8 8.95999999999992
11 9 555 9 7.07000000000005
12 10 561.18 10 6.17999999999995
13 11 566.99 11 5.81000000000006
14 12 573.43 12 6.43999999999994
15 13 577.98 13 4.55000000000007
16 14 583.67 14 5.68999999999994
17 15 590.4 15 6.73000000000002
18 16 599.08 16 8.68000000000006
19 17 611.88 17 12.8
20 18 630.02 18 18.14
21 19 656.96 19 26.9400000000001
22 20 697.33 20 40.37
23 21 753.98 21 56.65
24 22 789.21 22 35.23
25 23 796.87 23 7.65999999999997
26 24 796.78 24 -0.090000000000032

View File

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