merge

controller_teensy/src/main.cpp

@@ -23,7 +23,7 @@ unsigned long last_log_send=0;
 #define LOGMININTERVAL 20 //minimum interval (ms) to send logs
 #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;
 
 
@@ -57,6 +57,14 @@ unsigned long brake_ok_time=0;
 bool error_throttle_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_LED_START 2 //Enginge start led
 
@@ -112,12 +120,12 @@ SerialCommand CommandFront;
 SerialCommand CommandRear;
 
 
-typedef struct{
+typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
    uint16_t start;
    int16_t  cmd1;
    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  boardTemp;
    int16_t  curL_DC; //negative values are current consumed. positive values mean generated current
@@ -130,8 +138,8 @@ SerialFeedback NewFeedbackFront;
 SerialFeedback FeedbackRear;
 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{
   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};
@@ -285,6 +293,13 @@ void loop() {
     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
     last_adcread=loopmillis;
@@ -341,9 +356,9 @@ float filterMedian(int16_t* values) {
 void writeLogHeader(HardwareSerial &SerialRef) {
   SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_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.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)
@@ -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_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(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(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) 
@@ -465,7 +482,7 @@ void failChecks() {
 
 void sendCMD() {
   #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
   #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
@@ -569,7 +586,7 @@ void readButtons() {
 
   if (button_start_state) { //pressed
     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
         writeLogComment(Serial1,loopmillis, "Armed by button");
       }      

hoverboard-firmware-hack-foc-serial-esc

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

logdata_visualization/Timeline.pde

@@ -47,7 +47,7 @@ public class Timeline
       
       while ((cID+1 < logdata.getRowCount()) && ( t2 > (logdata.getRow(cID+1).getFloat("time")))) { //while endtime not reached
         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;
         

logdata_visualization/analyze.py

@@ -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()
+'''
+

logdata_visualization/logdata_visualization.pde

@@ -62,14 +62,17 @@ int timeoffset=0; //for moving timeslider
 Table logdata;
 int nextID=0; //next row number to be displayed
 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;
 
 int dataErrorCount=0;
 
+boolean running=true;
+int timePaused=0;
+
 //Data from log
 int cmd_FrontL;
 int cmd_FrontR;
@@ -114,20 +117,33 @@ void setup() {
     serialString = null;
   }else{
     logdata = loadTable(logfile_name, "header, csv");
+    float _checkTimeLast=-100;
+    
+    
     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)
-          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();
-          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_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);
@@ -247,15 +263,22 @@ void setup() {
   
   
   if (showTimeline) {
+    println("Preparing Timeline");
     tl = new Timeline(30,height-30, width-30*2, 28);
     tl.setTimes(logdata_start_time,logdata_end_time);
     tl.generatePreview(logdata);
+    println("Timeline prepared");
   }
   
 }
 
 void draw() {
-  long loopmillis=millis()-timeoffset;
+  int loopmillis=0;
+  if (running) {
+    loopmillis=millis()-timeoffset;
+  }else{ //paused
+    loopmillis=timePaused-timeoffset;
+  }
   
   if (useSerial) {
     if (serial.available() > 0) {
@@ -303,7 +326,7 @@ void draw() {
         TableRow row = logdata.getRow(nextID);
         
         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;
         nextTimeMillis=loopmillis;
@@ -315,20 +338,20 @@ void draw() {
         current_FrontL=row.getFloat("current_FrontL");
         current_FrontR=row.getFloat("current_FrontR");
         current_RearL=row.getFloat("current_RearL");
-      current_RearR=row.getFloat("current_RearR");
+        current_RearR=row.getFloat("current_RearR");
-      rpm_FrontL=row.getInt("rpm_FrontL");
+        rpm_FrontL=row.getInt("rpm_FrontL");
-      rpm_FrontR=row.getInt("rpm_FrontR");
+        rpm_FrontR=row.getInt("rpm_FrontR");
-      rpm_RearL=row.getInt("rpm_RearL");
+        rpm_RearL=row.getInt("rpm_RearL");
-      rpm_RearR=row.getInt("rpm_RearR");
+        rpm_RearR=row.getInt("rpm_RearR");
-      temp_Front=row.getFloat("temp_Front");
+        temp_Front=row.getFloat("temp_Front");
-      temp_Rear=row.getFloat("temp_Rear");
+        temp_Rear=row.getFloat("temp_Rear");
-      vbat_Front=row.getFloat("vbat_Front");
+        vbat_Front=row.getFloat("vbat_Front");
-      vbat_Rear=row.getFloat("vbat_Rear");
+        vbat_Rear=row.getFloat("vbat_Rear");
-      currentAll=row.getFloat("currentAll");    
+        currentAll=row.getFloat("currentAll");    
-      throttle=row.getInt("throttle");
+        throttle=row.getInt("throttle");
-      brake=row.getInt("brake");
+        brake=row.getInt("brake");
-      speed=row.getFloat("speed");
+        speed=row.getFloat("speed");
-      trip=row.getFloat("trip");
+        trip=row.getFloat("trip");
           
       if (loopmillis-nextTimeData>1000 && nextTimeData>lastTimeData) {//too much behind
         long _timestep=nextTimeData-lastTimeData; //approximated time step
@@ -407,8 +430,14 @@ void draw() {
   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
   }
-  
+  if (!running) {
+    fill(color(255,100,100));
+  }
   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);
   
@@ -423,12 +452,15 @@ void draw() {
 
 void keyPressed() {
   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 {
     //println("key="+keyCode);
     if (keyCode==82) { //82=r
@@ -437,6 +469,14 @@ void keyPressed() {
       nextID=0;
       newdataforced=true;
     }
+    if (keyCode == 32) {
+      if (running) { //switching from running to pause
+        timePaused=millis();
+      }else{ //unpause
+        timeoffset=+millis()-timePaused+timeoffset;
+      }
+      running=!running;
+    }
   }
 }