linearize throttle
This commit is contained in:
parent
f252d595b0
commit
7975ca4c7d
|
@ -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
|
|
|
@ -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
|
|
|
@ -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…
Reference in New Issue