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

add adc failsafe check

This commit is contained in:
interfisch 2021-03-31 21:50:30 +02:00
parent 1e9eda245f
commit fd3a474f18
1 changed files with 78 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
controller_teensy/src/main.cpp
View File

@ -36,15 +36,26 @@ bool controllers_connected=false;
#define PIN_THROTTLE A7
const uint16_t calib_throttle_min = 400; //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
#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;
const uint16_t failsafe_brake_min = 20; //if adc value falls below this failsafe is triggered
const uint16_t failsafe_brake_max = 1000; //if adc value goes above this failsafe is triggered
int16_t throttle_pos=0;
int16_t brake_pos=0;
unsigned long last_adcread=0;
#define ADCREADPERIOD 10
uint16_t throttle_raw=0;
uint16_t brake_raw=0;
#define ADC_OUTOFRANGE_TIME 100
unsigned long throttle_ok_time=0;
unsigned long brake_ok_time=0;
bool error_throttle_outofrange=false;
bool error_brake_outofrange=false;
#define PIN_START A9
#define PIN_LED_START 2 //Enginge start led
@ -137,6 +148,7 @@ void readADC();
void failChecks();
void sendCMD();
void checkLog();
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
{
@ -251,6 +263,13 @@ void loop() {
bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
if (newData2) {
updateMotorparams(motorparamsFront,FeedbackFront);
}
if (newData3) {
updateMotorparams(motorparamsRear,FeedbackRear);
}
if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
last_adcread=loopmillis;
@ -258,39 +277,22 @@ void loop() {
}
failChecks();
if (newData2) {
motorparamsFront.cur_pos++;
motorparamsFront.cur_pos%=CURRENT_FILTER_SIZE;
motorparamsFront.curL_DC[motorparamsFront.cur_pos] = -FeedbackFront.curL_DC; //invert so positive current is consumed current. negative then means regenerated
motorparamsFront.curR_DC[motorparamsFront.cur_pos] = -FeedbackFront.curR_DC;
motorparamsFront.millis=loopmillis;
log_update=true;
}
if (newData3) {
motorparamsRear.cur_pos++;
motorparamsRear.cur_pos%=CURRENT_FILTER_SIZE;
motorparamsRear.curL_DC[motorparamsRear.cur_pos] = -FeedbackRear.curL_DC;
motorparamsRear.curR_DC[motorparamsRear.cur_pos] = -FeedbackRear.curR_DC;
motorparamsRear.millis=loopmillis;
log_update=true;
}
failChecks();
if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers
last_send=loopmillis;
sendCMD();
}
// ######## LOG ########
//If needed write log to serial port
checkLog();
}
// ##### HELPFUNCTIONS
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
float a = *((float *)cmp1);
@ -360,27 +362,30 @@ void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg)
// #### LOOPFUNCTIONS
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
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
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
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();
//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
}
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() {
@ -406,7 +411,29 @@ void failChecks() {
controllers_connected=controllerFront_connected & controllerRear_connected;
if (!controllers_connected) { //controllers not available
//ADC Range Check
if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //outside safe range. maybe wire got disconnected
throttle_ok_time=loopmillis;
}
if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
if (!error_throttle_outofrange) {
error_throttle_outofrange=true;
writeLogComment(Serial1,loopmillis, "Error Throttle ADC Out of Range");
}
}
if ((brake_raw >= failsafe_brake_min) & (brake_raw <= failsafe_brake_max)) { //outside safe range. maybe wire got disconnected
brake_ok_time=loopmillis;
}
if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
if(!error_brake_outofrange) {
error_brake_outofrange=true;
writeLogComment(Serial1,loopmillis, "Error Brake ADC Out of Range");
}
}
if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange) { //controllers not available
throttle_pos=0;
brake_pos=0;
}
@ -472,4 +499,13 @@ void checkLog() {
log_update=false;
writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
}
}
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb) {
mp.cur_pos++;
mp.cur_pos%=CURRENT_FILTER_SIZE;
mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
mp.millis=loopmillis;
log_update=true;
}