interfisch
/
bobbycar
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix logging

This commit is contained in:
interfisch 2021-03-20 23:03:13 +01:00
parent 4d191648be
commit 378832ecb6
1 changed files with 22 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
controller_teensy/src/main.cpp
View File

@ -19,7 +19,8 @@ Tennsy Pin, Pin Name, Connected to
bool log_update=true; bool log_update=true;
unsigned long last_log_send=0; unsigned long last_log_send=0;
#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial #define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
bool controllers_connected=false; bool controllers_connected=false;
@ -34,7 +35,7 @@ int16_t throttle_pos=0;
int16_t brake_pos=0; int16_t brake_pos=0;
unsigned long last_adcread=0; unsigned long last_adcread=0;
#define ADCREADPERIOD 25 #define ADCREADPERIOD 10
#define PIN_START A9 #define PIN_START A9
#define PIN_LED_START 2 //Enginge start led #define PIN_LED_START 2 //Enginge start led
@ -244,9 +245,9 @@ void loop() {
uint16_t brake_raw = analogRead(PIN_BRAKE); uint16_t 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=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
int16_t throttlebreak_pos = throttle_pos-brake_pos*2; int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
throttle_pos=constrain(throttlebreak_pos,0,1000); //reduce throttle_when applying brake throttle_pos=constrain(throttlebreak_pos,0,1000);
brake_pos=constrain(-throttlebreak_pos/2,0,1000); brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos
//Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", "); //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
//Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println(); //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
@ -295,7 +296,7 @@ void loop() {
last_send=loopmillis; last_send=loopmillis;
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
#define MAXBREAKCURRENT 20 #define MAXBREAKCURRENT 5
float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000) float brakepedal_current_multiplier=MAXBREAKCURRENT/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)
float freewheel_current=0.1+brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling float freewheel_current=0.1+brake_pos*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
@ -319,7 +320,11 @@ void loop() {
cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
cmd_send=constrain(cmd_send,0,1000); cmd_send=constrain(cmd_send,0,1000);
} }
if (!controllers_connected) { //controllers not connected
cmd_send=0; //safety off
}
last_cmd_send=cmd_send; last_cmd_send=cmd_send;
//apply throttle command to all motors //apply throttle command to all motors
@ -328,15 +333,17 @@ void loop() {
motorparamsRear.cmdL=cmd_send; motorparamsRear.cmdL=cmd_send;
motorparamsRear.cmdR=cmd_send; motorparamsRear.cmdR=cmd_send;
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2); if (controllers_connected) {
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3); SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
log_update=true; 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(); //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()
}
} }
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
if (log_update && loopmillis>last_log_send+LOGMININTERVAL) { if (log_update && loopmillis>last_log_send+LOGMININTERVAL) {
last_log_send=loopmillis; last_log_send=loopmillis;
log_update=false; log_update=false;
@ -380,7 +387,7 @@ void writeLogHeader(HardwareSerial &SerialRef) {
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)
{ {
SerialRef.print(time/1000.0); SerialRef.print(","); //time in seconds SerialRef.print(time/1000.0,3); SerialRef.print(","); //time in seconds
SerialRef.print(mpfront.cmdL); SerialRef.print(","); SerialRef.print(mpfront.cmdL); SerialRef.print(",");
SerialRef.print(mpfront.cmdR); SerialRef.print(","); SerialRef.print(mpfront.cmdR); SerialRef.print(",");
SerialRef.print(mprear.cmdL); SerialRef.print(","); SerialRef.print(mprear.cmdL); SerialRef.print(",");
@ -398,8 +405,8 @@ void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpf
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
SerialRef.print(fbfront.batVoltage); SerialRef.print(","); SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(fbrear.batVoltage); SerialRef.print(","); SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V
SerialRef.print(currentAll,3); SerialRef.print(","); SerialRef.print(currentAll,3); SerialRef.print(",");
SerialRef.print(throttle); SerialRef.print(","); SerialRef.print(throttle); SerialRef.print(",");