From 2991bf8c0782b73fcc535dd411e883c9acf02cb6 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sat, 3 Jun 2023 12:50:29 +0200 Subject: [PATCH] fix current reading and fix throttle premature failsafe --- controller_teensy/include/definitions.h | 5 ++-- controller_teensy/include/display.h | 14 ++++++++++ controller_teensy/src/main.cpp | 35 ++++++++++++++++++++++++- 3 files changed, 51 insertions(+), 3 deletions(-) diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index 7f97f44..2a832b0 100644 --- a/controller_teensy/include/definitions.h +++ b/controller_teensy/include/definitions.h @@ -55,7 +55,7 @@ unsigned long loopmillis; #define BUTTONREADPERIOD 20 unsigned long last_adsread=0; //needed for failcheck uint16_t throttle_raw=failsafe_throttle_min; //start at min so that failsafe is not triggered -#define THROTTLE_ADC_FILTER 0.15 //higher value = faster response +#define THROTTLE_ADC_FILTER 0.4 //higher value = faster response uint16_t brake_raw=failsafe_brake_min; //start at min so that failsafe is not triggered #define ADC_OUTOFRANGE_TIME 100 unsigned long throttle_ok_time=0; @@ -76,7 +76,7 @@ int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send i int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_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) float startbrakecurrent=2; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor -float startbrakecurrent_offset=0.15; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading +float startbrakecurrent_offset=0.13; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading bool reverse_enabled=false; unsigned long last_notidle=0; //not rolling to fast, no pedal pressed @@ -89,6 +89,7 @@ unsigned long last_notidle=0; //not rolling to fast, no pedal pressed #define PIN_MODE_LEDG 4 #define PIN_MODE_LEDR 5 +#define PIN_FAN 6 //Output High=Fans on //unsigned long last_send = 0; diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h index 53b0e67..daf27db 100644 --- a/controller_teensy/include/display.h +++ b/controller_teensy/include/display.h @@ -119,6 +119,20 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { display.setCursor(SCREEN_WIDTH-25,SCREEN_HEIGHT-16); display.print("km/h"); + //A + + display.setCursor(SCREEN_WIDTH-30,1); + static float averaged_filtered_currentAll; + #define CURRENT_FILTER 0.95 + averaged_filtered_currentAll=averaged_filtered_currentAll*CURRENT_FILTER+(1-CURRENT_FILTER)*filtered_currentAll; //filter over time + dtostrf(averaged_filtered_currentAll,1,2,buf); + strbuf=buf; + if (strbuf.length()<5) { //pad spaces on the left + strbuf=" "+strbuf; + } + display.print(strbuf); + display.print("A"); + //## Trip / Current Consumed Display display.setCursor(1,SCREEN_HEIGHT-7); diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 10cb875..1a4fbdb 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -73,6 +73,9 @@ void setup() pinMode(PIN_START, INPUT_PULLUP); //Pressed=High pinMode(PIN_LED_START, OUTPUT); //Active High + + pinMode(PIN_FAN,OUTPUT); + digitalWrite(PIN_FAN,HIGH); //Turn fan on during startup for debugging purposes //TODO: remove mode button things @@ -125,6 +128,10 @@ void setup() led_simpeProgress(6,true); } delay(10); + for (uint8_t i=0;i<4;i++){ //read all channels once to have adc readings ready in first loop (to prevent premature failsafe) + readADS(); + delay(10); + } setSyncProvider(getTeensy3Time); //See https://www.pjrc.com/teensy/td_libs_Time.html#teensy3 @@ -269,6 +276,32 @@ void loop() { last_display_update=loopmillis; display_update(escFront,escRear); } + + //Fan + static unsigned long last_fan_update=0; + #define FANUPDATEPERIOD 5000 + float fan_turn_on_temp=45; + float fan_turn_off_temp=32; + if (loopmillis - last_fan_update > FANUPDATEPERIOD) { + last_fan_update=loopmillis; + boolean fanstatus=digitalRead(PIN_FAN); + float temp=max(escFront.getFeedback_boardTemp(),escRear.getFeedback_boardTemp()); + if (!escFront.getControllerConnected() || !escRear.getControllerConnected()) { //boards are not powered on + digitalWrite(PIN_FAN,HIGH); //force fan on + }else{ //if both controllers are on, use temperature regulation + if (!fanstatus) { //fan is off + if (temp>=fan_turn_on_temp){ + digitalWrite(PIN_FAN,HIGH); + } + }else{ //fan is on + if (temp<=fan_turn_off_temp){ + digitalWrite(PIN_FAN,LOW); + } + } + } + + } + } @@ -320,7 +353,7 @@ void readADC() { Serial.print(ads_throttle_B_raw); Serial.print('\t'); Serial.print(ads_brake_raw); Serial.print('\t'); Serial.print(ads_control_raw); Serial.println();*/ - throttle_raw = ads_throttle_A_raw*THROTTLE_ADC_FILTER + throttle_raw*(1-THROTTLE_ADC_FILTER); //apply filter + throttle_raw = (ads_throttle_A_raw+ads_throttle_B_raw)/2.0*THROTTLE_ADC_FILTER + throttle_raw*(1-THROTTLE_ADC_FILTER); //apply filter //maps throttle curve to be linear throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain