This commit is contained in:
interfisch 2021-05-28 19:52:26 +02:00
commit dc3b7d6ff5
5 changed files with 157 additions and 48 deletions

View File

@ -23,7 +23,7 @@ unsigned long last_log_send=0;
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs #define LOGMININTERVAL 20 //minimum interval (ms) to send logs
#define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send #define LOGMAXINTERVAL 10000 //maximum time (ms) after which data is send
#define WRITE_HEADER_TIME 1000 #define WRITE_HEADER_TIME 400 //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments
bool log_header_written = false; bool log_header_written = false;
@ -57,6 +57,14 @@ unsigned long brake_ok_time=0;
bool error_throttle_outofrange=false; bool error_throttle_outofrange=false;
bool error_brake_outofrange=false; bool error_brake_outofrange=false;
unsigned long last_feedback=0; //time millis at last feedback receive
float meanSpeedms=0;
float trip=0; //trip distance in meters
float wheelcircumference=0.5278; //wheel diameter in m. 8.4cm radius -> 0.084m*2*Pi
#define PIN_START A9 #define PIN_START A9
#define PIN_LED_START 2 //Enginge start led #define PIN_LED_START 2 //Enginge start led
@ -112,12 +120,12 @@ SerialCommand CommandFront;
SerialCommand CommandRear; SerialCommand CommandRear;
typedef struct{ typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
uint16_t start; uint16_t start;
int16_t cmd1; int16_t cmd1;
int16_t cmd2; int16_t cmd2;
int16_t speedL_meas; //left speed is negative when driving forward int16_t speedL_meas; //left speed is positive when driving forward
int16_t speedR_meas; //right speed is positive when driving forward int16_t speedR_meas; //right speed is negatie when driving forward
int16_t batVoltage; int16_t batVoltage;
int16_t boardTemp; int16_t boardTemp;
int16_t curL_DC; //negative values are current consumed. positive values mean generated current int16_t curL_DC; //negative values are current consumed. positive values mean generated current
@ -130,8 +138,8 @@ SerialFeedback NewFeedbackFront;
SerialFeedback FeedbackRear; SerialFeedback FeedbackRear;
SerialFeedback NewFeedbackRear; SerialFeedback NewFeedbackRear;
#define CURRENT_FILTER_SIZE 20 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller) #define CURRENT_FILTER_SIZE 40 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
#define CURRENT_MEANVALUECOUNT 4 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used #define CURRENT_MEANVALUECOUNT 10 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used
typedef struct{ typedef struct{
int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current
int16_t curR_DC[CURRENT_FILTER_SIZE] = {0}; int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
@ -285,6 +293,13 @@ void loop() {
updateMotorparams(motorparamsRear,FeedbackRear); updateMotorparams(motorparamsRear,FeedbackRear);
} }
if (newData2 || newData3) { //when new data arrived from one controller
float _meanRPM=FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas;
float meanSpeedms=_meanRPM*wheelcircumference/60.0; // Units: 1/min * m / 60s
trip+=abs(meanSpeedms)* (millis()-last_feedback) / 1000.0;
last_feedback=millis();
}
if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
last_adcread=loopmillis; last_adcread=loopmillis;
@ -341,9 +356,9 @@ float filterMedian(int16_t* values) {
void writeLogHeader(HardwareSerial &SerialRef) { void writeLogHeader(HardwareSerial &SerialRef) {
SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,"); SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_RearR,"); SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
SerialRef.print("speed_FrontL,speed_FrontR,speed_RearL,speed_RearR,"); SerialRef.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,"); SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
SerialRef.println("currentAll,throttle,brake"); SerialRef.println("currentAll,throttle,brake,speed,trip");
} }
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake) void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
@ -359,10 +374,10 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
SerialRef.print(mprear.filtered_curL,3); SerialRef.print(","); SerialRef.print(mprear.filtered_curL,3); SerialRef.print(",");
SerialRef.print(mprear.filtered_curR,3); SerialRef.print(","); SerialRef.print(mprear.filtered_curR,3); SerialRef.print(",");
SerialRef.print(-fbfront.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated SerialRef.print(fbfront.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
SerialRef.print(fbfront.speedR_meas); SerialRef.print(","); SerialRef.print(-fbfront.speedR_meas); SerialRef.print(",");
SerialRef.print(-fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
SerialRef.print(fbrear.speedR_meas); SerialRef.print(","); SerialRef.print(-fbrear.speedR_meas); SerialRef.print(",");
SerialRef.print(fbfront.boardTemp/10.0); SerialRef.print(","); //in degC SerialRef.print(fbfront.boardTemp/10.0); SerialRef.print(","); //in degC
SerialRef.print(fbrear.boardTemp/10.0); SerialRef.print(","); //in degC SerialRef.print(fbrear.boardTemp/10.0); SerialRef.print(","); //in degC
@ -371,7 +386,9 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
SerialRef.print(currentAll,3); SerialRef.print(","); //invert. positive current is drive current SerialRef.print(currentAll,3); SerialRef.print(","); //invert. positive current is drive current
SerialRef.print(throttle); SerialRef.print(","); SerialRef.print(throttle); SerialRef.print(",");
SerialRef.print(brake); SerialRef.println(); SerialRef.print(brake); SerialRef.print(",");
SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
SerialRef.print(trip); SerialRef.println(); //in m
} }
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg) void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
@ -465,7 +482,7 @@ void failChecks() {
void sendCMD() { void sendCMD() {
#define MINIMUM_CONSTANT_CMD_REDUCE 1 //reduce cmd every loop by this constant amount when freewheeling/braking #define MINIMUM_CONSTANT_CMD_REDUCE 1 //reduce cmd every loop by this constant amount when freewheeling/braking
#define BREAK_CMDREDUCE_PROPORTIONAL 500 //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=BREAK_CMDREDUCE_PROPORTIONAL / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling) #define BREAK_CMDREDUCE_PROPORTIONAL 100 //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=BREAK_CMDREDUCE_PROPORTIONAL / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,(int16_t)(BREAK_CMDREDUCE_PROPORTIONAL*SENDPERIOD/1000)); //reduce cmd value every cycle int16_t cmdreduce_constant=map(brake_pos,0,1000,0,(int16_t)(BREAK_CMDREDUCE_PROPORTIONAL*SENDPERIOD/1000)); //reduce cmd value every cycle
#define STARTBREAKCURRENT 5 //Ampere. at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor #define STARTBREAKCURRENT 5 //Ampere. at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
#define STARTBREAKCURRENT_OFFSET 0.1 //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading #define STARTBREAKCURRENT_OFFSET 0.1 //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
@ -569,7 +586,7 @@ void readButtons() {
if (button_start_state) { //pressed if (button_start_state) { //pressed
if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
if (throttle_pos<=0 && brake_pos<=0 && !armed) { //brake or thottle not pressed if (throttle_pos<=0 && brake_pos<=0 && controllers_connected && !armed) { //brake or thottle not pressed, controllers connected
armed=true; //arm if button pressed long enough armed=true; //arm if button pressed long enough
writeLogComment(Serial1,loopmillis, "Armed by button"); writeLogComment(Serial1,loopmillis, "Armed by button");
} }

@ -1 +1 @@
Subproject commit 898898a63c6b494e0525df8a8ed2da98fcd5cdb1 Subproject commit 40eb4916f98447f23c28058e31cd867cbc8f0e6c

View File

@ -47,7 +47,7 @@ public class Timeline
while ((cID+1 < logdata.getRowCount()) && ( t2 > (logdata.getRow(cID+1).getFloat("time")))) { //while endtime not reached while ((cID+1 < logdata.getRowCount()) && ( t2 > (logdata.getRow(cID+1).getFloat("time")))) { //while endtime not reached
TableRow row = logdata.getRow(cID); TableRow row = logdata.getRow(cID);
nextTimeData=(long)(logdata.getRow(cID+1).getFloat("time")*1000); //get time and convert from seconds to ms nextTimeData=(int)(logdata.getRow(cID+1).getFloat("time")*1000); //get time and convert from seconds to ms
lastTimeMillis=nextTimeMillis; lastTimeMillis=nextTimeMillis;

View File

@ -0,0 +1,52 @@
import matplotlib.pyplot as plt
import csv
x=[]
speed_FrontL=[]
speed_FrontR=[]
speed_RearL=[]
speed_RearR=[]
fp = open('LOG00193_20210516_fixedSpeedLR_freewheelingTest.TXT')
rdr = csv.DictReader(filter(lambda row: row[0]!='#', fp))
for row in rdr:
#print(row)
x.append(float(row['cmd_FrontL']))
speed_FrontL.append(float(row['speed_FrontL']))
speed_FrontR.append(float(row['speed_FrontR']))
speed_RearL.append(float(row['speed_RearL']))
speed_RearR.append(float(row['speed_RearR']))
fp.close()
#plt.plot(x,y, label='Loaded from file!')
scattersize=5
scatteralpha=0.1
plt.scatter(x,speed_FrontL, s=scattersize, alpha=scatteralpha, label="speed_FrontL")
plt.scatter(x,speed_FrontR, s=scattersize, alpha=scatteralpha, label="speed_FrontR")
plt.scatter(x,speed_RearL, s=scattersize, alpha=scatteralpha, label="speed_RearL")
plt.scatter(x,speed_RearR, s=scattersize, alpha=scatteralpha, label="speed_RearR")
plt.xlabel('cmd')
plt.ylabel('speed')
plt.title('Interesting Graph\nCheck it out')
plt.legend()
plt.show()
'''
with open(,'r') as csvfile:
plots = csv.reader(filter(lambda row: row[0]!='#', csvfile), delimiter=',')
for row in plots:
x.append(float(row[0]))
y.append(float(row[1]))
plt.plot(x,y, label='Loaded from file!')
plt.xlabel('x')
plt.ylabel('y')
plt.title('Interesting Graph\nCheck it out')
plt.legend()
plt.show()
'''

View File

@ -62,14 +62,17 @@ int timeoffset=0; //for moving timeslider
Table logdata; Table logdata;
int nextID=0; //next row number to be displayed int nextID=0; //next row number to be displayed
long lastTimeData=0; //last time data received long lastTimeData=0; //last time data received
long nextTimeData=0; //time of nextID row int nextTimeData=0; //time of nextID row
long lastTimeMillis=0; //local time int lastTimeMillis=0; //local time
long nextTimeMillis=0; //local time int nextTimeMillis=0; //local time
boolean newdataforced=true; boolean newdataforced=true;
int dataErrorCount=0; int dataErrorCount=0;
boolean running=true;
int timePaused=0;
//Data from log //Data from log
int cmd_FrontL; int cmd_FrontL;
int cmd_FrontR; int cmd_FrontR;
@ -114,20 +117,33 @@ void setup() {
serialString = null; serialString = null;
}else{ }else{
logdata = loadTable(logfile_name, "header, csv"); logdata = loadTable(logfile_name, "header, csv");
float _checkTimeLast=-100;
for (int i=0; i < logdata.getRowCount();i++) { for (int i=0; i < logdata.getRowCount();i++) {
float _checkTimeCurrent=logdata.getRow(i).getFloat("time")*1000;
if (logdata.getRow(i).getString(0).charAt(0)=='#') { //check if row starts with # (comment) if (logdata.getRow(i).getString(0).charAt(0)=='#') { //check if row starts with # (comment)
print("removed row:"); print("removed comment:");
for (int is=0;is< logdata.getRow(i).getColumnCount(); is++) for (int is=0;is< logdata.getRow(i).getColumnCount() && logdata.getRow(i).getString(is) != null; is++)
{ {
print(logdata.getRow(i).getString(is)+","); print(", "+logdata.getRow(i).getString(is));
} }
println(); println();
logdata.removeRow(i); logdata.removeRow(i); i--;
}else if (Float.isNaN(_checkTimeCurrent) || _checkTimeCurrent <= _checkTimeLast) { //check if time is plausible
print("removed unplausible time:");
for (int is=0;is< logdata.getRow(i).getColumnCount() && logdata.getRow(i).getString(is) != null; is++)
{
print(", "+logdata.getRow(i).getString(is));
}
println();
logdata.removeRow(i); i--;
} }
} }
logdata_start_time=logdata.getRow(0).getFloat("time"); logdata_start_time=logdata.getRow(0).getFloat("time");
logdata_end_time = logdata.getRow(logdata.getRowCount()-1).getFloat("time"); logdata_end_time = logdata.getRow(logdata.getRowCount()-1).getFloat("time");
println("loaded "+logdata.getRowCount()+" lines. Times: "+logdata_start_time+"s to "+logdata_end_time+"s"); println("loaded "+logdata.getRowCount()+" lines. Starttime: "+logdata_start_time+"s , Endtime: "+logdata_end_time+"s");
} }
PVector pos_vis_cmd = new PVector(100,200); PVector pos_vis_cmd = new PVector(100,200);
@ -247,15 +263,22 @@ void setup() {
if (showTimeline) { if (showTimeline) {
println("Preparing Timeline");
tl = new Timeline(30,height-30, width-30*2, 28); tl = new Timeline(30,height-30, width-30*2, 28);
tl.setTimes(logdata_start_time,logdata_end_time); tl.setTimes(logdata_start_time,logdata_end_time);
tl.generatePreview(logdata); tl.generatePreview(logdata);
println("Timeline prepared");
} }
} }
void draw() { void draw() {
long loopmillis=millis()-timeoffset; int loopmillis=0;
if (running) {
loopmillis=millis()-timeoffset;
}else{ //paused
loopmillis=timePaused-timeoffset;
}
if (useSerial) { if (useSerial) {
if (serial.available() > 0) { if (serial.available() > 0) {
@ -303,7 +326,7 @@ void draw() {
TableRow row = logdata.getRow(nextID); TableRow row = logdata.getRow(nextID);
lastTimeData=nextTimeData; lastTimeData=nextTimeData;
nextTimeData=(long)(logdata.getRow(nextID+1).getFloat("time")*1000); //get time and convert from seconds to ms nextTimeData=(int)(logdata.getRow(nextID+1).getFloat("time")*1000); //get time and convert from seconds to ms
lastTimeMillis=nextTimeMillis; lastTimeMillis=nextTimeMillis;
nextTimeMillis=loopmillis; nextTimeMillis=loopmillis;
@ -407,8 +430,14 @@ void draw() {
if (!useSerial && loopmillis-lastTimeData>(nextTimeData-lastTimeData)*10) { //deviation too high when reading from file if (!useSerial && loopmillis-lastTimeData>(nextTimeData-lastTimeData)*10) { //deviation too high when reading from file
text("ff="+(loopmillis-lastTimeData)+"ms", 5+75*2,12); //show warning text("ff="+(loopmillis-lastTimeData)+"ms", 5+75*2,12); //show warning
} }
if (!running) {
fill(color(255,100,100));
}
text("t="+(loopmillis/1000.0)+"s", 5,12); text("t="+(loopmillis/1000.0)+"s", 5,12);
text("nextID="+nextID, 5,12+12);
fill(color(200,200,200));
text(""+(dataErrorCount)+" errors", 5+70*3,12); text(""+(dataErrorCount)+" errors", 5+70*3,12);
@ -423,12 +452,15 @@ void draw() {
void keyPressed() { void keyPressed() {
if (key == CODED) { if (key == CODED) {
/*if (keyCode == UP) { if (!running) { //paused
fillVal = 255; if (keyCode == LEFT) {
} else if (keyCode == DOWN) {
fillVal = 0; } else if (keyCode == RIGHT) {
timeoffset=timePaused-nextTimeData;
} }
*/ }
} else { } else {
//println("key="+keyCode); //println("key="+keyCode);
if (keyCode==82) { //82=r if (keyCode==82) { //82=r
@ -437,6 +469,14 @@ void keyPressed() {
nextID=0; nextID=0;
newdataforced=true; newdataforced=true;
} }
if (keyCode == 32) {
if (running) { //switching from running to pause
timePaused=millis();
}else{ //unpause
timeoffset=+millis()-timePaused+timeoffset;
}
running=!running;
}
} }
} }