move functions in loop to own functions
This commit is contained in:
parent
82fbc8234e
commit
1e9eda245f
|
@ -26,6 +26,11 @@ unsigned long last_log_send=0;
|
||||||
#define WRITE_HEADER_TIME 1000
|
#define WRITE_HEADER_TIME 1000
|
||||||
bool log_header_written = false;
|
bool log_header_written = false;
|
||||||
|
|
||||||
|
|
||||||
|
#define FEEDBACKRECEIVETIMEOUT 500
|
||||||
|
|
||||||
|
bool controllerFront_connected=false;
|
||||||
|
bool controllerRear_connected=false;
|
||||||
bool controllers_connected=false;
|
bool controllers_connected=false;
|
||||||
|
|
||||||
#define PIN_THROTTLE A7
|
#define PIN_THROTTLE A7
|
||||||
|
@ -126,6 +131,12 @@ float filterMedian(int16_t* values);
|
||||||
|
|
||||||
void writeLogHeader(HardwareSerial &SerialRef);
|
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);
|
||||||
|
void writeLogComment(HardwareSerial &SerialRef, String msg);
|
||||||
|
|
||||||
|
void readADC();
|
||||||
|
void failChecks();
|
||||||
|
void sendCMD();
|
||||||
|
void checkLog();
|
||||||
|
|
||||||
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
|
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
|
||||||
{
|
{
|
||||||
|
@ -241,42 +252,14 @@ void loop() {
|
||||||
bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
|
bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
|
||||||
|
|
||||||
|
|
||||||
if (loopmillis - last_adcread > ADCREADPERIOD) {
|
if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
|
||||||
//read teensy adc and filter
|
|
||||||
last_adcread=loopmillis;
|
last_adcread=loopmillis;
|
||||||
uint16_t throttle_raw = analogRead(PIN_THROTTLE);
|
readADC();
|
||||||
throttle_pos=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000))); //map and constrain
|
|
||||||
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
|
|
||||||
|
|
||||||
int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
|
|
||||||
throttle_pos=constrain(throttlebreak_pos,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_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
|
|
||||||
|
|
||||||
if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
|
|
||||||
throttle_pos/=2;
|
|
||||||
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
|
|
||||||
digitalWrite(PIN_MODE_LEDR,HIGH);
|
|
||||||
}else{ //button not pushed in
|
|
||||||
digitalWrite(PIN_MODE_LEDG,HIGH);
|
|
||||||
digitalWrite(PIN_MODE_LEDR,LOW); //Red
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#define FEEDBACKRECEIVETIMEOUT 500
|
|
||||||
if ( (loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT) | (loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT) ) { //timeout of at least one controller
|
|
||||||
throttle_pos=0;
|
|
||||||
brake_pos=0;
|
|
||||||
controllers_connected=false;
|
|
||||||
}else if(!controllers_connected) { //not timeouted but was before
|
|
||||||
controllers_connected=true;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
failChecks();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@ -297,71 +280,15 @@ void loop() {
|
||||||
log_update=true;
|
log_update=true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (loopmillis - last_send > SENDPERIOD) {
|
if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers
|
||||||
//Calculate motor stuff and send to motor controllers
|
|
||||||
last_send=loopmillis;
|
last_send=loopmillis;
|
||||||
|
sendCMD();
|
||||||
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
|
|
||||||
#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 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_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
|
|
||||||
motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
|
|
||||||
motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
|
|
||||||
motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
|
|
||||||
motorparamsRear.filtered_curR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
|
|
||||||
|
|
||||||
float filtered_currentFront=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
|
|
||||||
float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
|
|
||||||
|
|
||||||
filtered_currentAll=max(filtered_currentFront,filtered_currentRear);
|
|
||||||
|
|
||||||
if (throttle_pos>=last_cmd_send) { //accelerating
|
|
||||||
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
|
|
||||||
}else{ //freewheeling or braking
|
|
||||||
if (filtered_currentAll<freewheel_current) { //drive current too high
|
|
||||||
cmd_send-= max(0, (-filtered_currentAll+freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
|
|
||||||
}
|
|
||||||
cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
|
|
||||||
cmd_send=constrain(cmd_send,0,1000);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!controllers_connected) { //controllers not connected
|
|
||||||
cmd_send=0; //safety off
|
|
||||||
}
|
|
||||||
|
|
||||||
last_cmd_send=cmd_send;
|
|
||||||
|
|
||||||
//apply throttle command to all motors
|
|
||||||
motorparamsFront.cmdL=cmd_send;
|
|
||||||
motorparamsFront.cmdR=cmd_send;
|
|
||||||
motorparamsRear.cmdL=cmd_send;
|
|
||||||
motorparamsRear.cmdR=cmd_send;
|
|
||||||
|
|
||||||
if (controllers_connected) {
|
|
||||||
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
|
||||||
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
|
|
||||||
|
|
||||||
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()
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// ######## LOG ########
|
// ######## LOG ########
|
||||||
|
|
||||||
if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){
|
checkLog();
|
||||||
writeLogHeader(Serial1); //connection recovered, write log header
|
|
||||||
log_header_written=true;
|
|
||||||
}
|
|
||||||
|
|
||||||
if ((log_header_written && log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) {
|
|
||||||
last_log_send=loopmillis;
|
|
||||||
log_update=false;
|
|
||||||
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
|
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
|
||||||
|
@ -424,4 +351,125 @@ 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.println();
|
||||||
|
}
|
||||||
|
|
||||||
|
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
|
||||||
|
{
|
||||||
|
SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void readADC() {
|
||||||
|
uint16_t 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
|
||||||
|
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
|
||||||
|
|
||||||
|
int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
|
||||||
|
throttle_pos=constrain(throttlebreak_pos,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_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();
|
||||||
|
|
||||||
|
if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
|
||||||
|
throttle_pos/=2;
|
||||||
|
digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
|
||||||
|
digitalWrite(PIN_MODE_LEDR,HIGH);
|
||||||
|
}else{ //button not pushed in
|
||||||
|
digitalWrite(PIN_MODE_LEDG,HIGH);
|
||||||
|
digitalWrite(PIN_MODE_LEDR,LOW); //Red
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void failChecks() {
|
||||||
|
if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected
|
||||||
|
if (controllerFront_connected) { //just got disconnected
|
||||||
|
controllerFront_connected=false;
|
||||||
|
writeLogComment(Serial1,loopmillis, "Controller Front feedback timeout");
|
||||||
|
}
|
||||||
|
}else if(!controllerFront_connected) { //not timeouted but was before
|
||||||
|
controllerFront_connected=true;
|
||||||
|
writeLogComment(Serial1,loopmillis, "Controller Front connected");
|
||||||
|
}
|
||||||
|
|
||||||
|
if ( loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT ) { //controller disconnected
|
||||||
|
if (controllerRear_connected) { //just got disconnected
|
||||||
|
controllerRear_connected=false;
|
||||||
|
writeLogComment(Serial1,loopmillis, "Controller Rear feedback timeout");
|
||||||
|
}
|
||||||
|
}else if(!controllerRear_connected) { //not timeouted but was before
|
||||||
|
controllerRear_connected=true;
|
||||||
|
writeLogComment(Serial1,loopmillis, "Controller Rear connected");
|
||||||
|
}
|
||||||
|
|
||||||
|
controllers_connected=controllerFront_connected & controllerRear_connected;
|
||||||
|
|
||||||
|
if (!controllers_connected) { //controllers not available
|
||||||
|
throttle_pos=0;
|
||||||
|
brake_pos=0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendCMD() {
|
||||||
|
int16_t cmdreduce_constant=map(brake_pos,0,1000,0,10); //reduce cmd value every cycle
|
||||||
|
#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 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_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (in average)
|
||||||
|
motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
|
||||||
|
motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
|
||||||
|
motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
|
||||||
|
motorparamsRear.filtered_curR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps
|
||||||
|
|
||||||
|
float filtered_currentFront=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
|
||||||
|
float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);
|
||||||
|
|
||||||
|
filtered_currentAll=max(filtered_currentFront,filtered_currentRear);
|
||||||
|
|
||||||
|
if (throttle_pos>=last_cmd_send) { //accelerating
|
||||||
|
cmd_send = throttle_pos; //if throttle higher than apply throttle directly
|
||||||
|
}else{ //freewheeling or braking
|
||||||
|
if (filtered_currentAll<freewheel_current) { //drive current too high
|
||||||
|
cmd_send-= max(0, (-filtered_currentAll+freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
|
||||||
|
}
|
||||||
|
cmd_send-=max(1,cmdreduce_constant); //reduce slowly anyways
|
||||||
|
cmd_send=constrain(cmd_send,0,1000);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!controllers_connected) { //controllers not connected
|
||||||
|
cmd_send=0; //safety off
|
||||||
|
}
|
||||||
|
|
||||||
|
last_cmd_send=cmd_send;
|
||||||
|
|
||||||
|
//apply throttle command to all motors
|
||||||
|
motorparamsFront.cmdL=cmd_send;
|
||||||
|
motorparamsFront.cmdR=cmd_send;
|
||||||
|
motorparamsRear.cmdL=cmd_send;
|
||||||
|
motorparamsRear.cmdR=cmd_send;
|
||||||
|
|
||||||
|
if (controllers_connected) {
|
||||||
|
SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
|
||||||
|
SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);
|
||||||
|
|
||||||
|
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()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void checkLog() {
|
||||||
|
if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){
|
||||||
|
writeLogHeader(Serial1); //connection recovered, write log header
|
||||||
|
log_header_written=true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((log_header_written && log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) {
|
||||||
|
last_log_send=loopmillis;
|
||||||
|
log_update=false;
|
||||||
|
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
|
||||||
|
}
|
||||||
}
|
}
|
Loading…
Reference in New Issue