From 2868ab0bd2e4102a58b7b749b1c21866c1b0907c Mon Sep 17 00:00:00 2001 From: Fisch Date: Sat, 21 Sep 2024 22:10:51 +0200 Subject: [PATCH] add remote control ppm input --- Bobbycar_wiring/Bobbycar_wiring.kicad_pro | 4 +- Bobbycar_wiring/Bobbycar_wiring.kicad_sch | 56 ++++++++-- controller_teensy/include/definitions.h | 3 +- controller_teensy/include/display.h | 44 +++++++- controller_teensy/include/led.h | 28 +++-- controller_teensy/include/rc.h | 124 ++++++++++++++++++++++ controller_teensy/platformio.ini | 1 + controller_teensy/src/main.cpp | 101 ++++++++++++++---- 8 files changed, 313 insertions(+), 48 deletions(-) create mode 100644 controller_teensy/include/rc.h diff --git a/Bobbycar_wiring/Bobbycar_wiring.kicad_pro b/Bobbycar_wiring/Bobbycar_wiring.kicad_pro index ba947f2..a5257c5 100644 --- a/Bobbycar_wiring/Bobbycar_wiring.kicad_pro +++ b/Bobbycar_wiring/Bobbycar_wiring.kicad_pro @@ -59,9 +59,7 @@ "width": 0.0 } ], - "drc_exclusions": [ - "lib_footprint_mismatch|93919000|108539000|df2709a9-a255-4f53-9496-85732562ad08|00000000-0000-0000-0000-000000000000" - ], + "drc_exclusions": [], "meta": { "version": 2 }, diff --git a/Bobbycar_wiring/Bobbycar_wiring.kicad_sch b/Bobbycar_wiring/Bobbycar_wiring.kicad_sch index 9db770f..0a3c11a 100644 --- a/Bobbycar_wiring/Bobbycar_wiring.kicad_sch +++ b/Bobbycar_wiring/Bobbycar_wiring.kicad_sch @@ -8693,7 +8693,7 @@ ) (wire (pts - (xy 209.55 31.75) (xy 209.55 33.655) + (xy 198.12 31.75) (xy 198.12 35.56) ) (stroke (width 0) @@ -9363,7 +9363,7 @@ ) (wire (pts - (xy 209.55 31.75) (xy 231.14 31.75) + (xy 198.12 31.75) (xy 231.14 31.75) ) (stroke (width 0) @@ -10551,6 +10551,16 @@ ) (uuid "bcfd5886-f378-4947-9815-f92d60528bcf") ) + (wire + (pts + (xy 201.93 92.71) (xy 201.93 34.29) + ) + (stroke + (width 0) + (type default) + ) + (uuid "beaa63fa-5d4b-4486-b3ce-f0f5f5c78409") + ) (wire (pts (xy 49.53 96.52) (xy 69.85 96.52) @@ -10821,6 +10831,16 @@ ) (uuid "dde8619c-5a8c-40eb-9845-65e6a654222d") ) + (wire + (pts + (xy 201.93 34.29) (xy 231.14 34.29) + ) + (stroke + (width 0) + (type default) + ) + (uuid "de2cede1-ffa3-413c-9082-8bda9711045c") + ) (wire (pts (xy 85.725 137.16) (xy 87.63 137.16) @@ -10991,6 +11011,16 @@ ) (uuid "e848cb4e-e9be-4642-b62e-cfdc7033410f") ) + (wire + (pts + (xy 215.9 92.71) (xy 201.93 92.71) + ) + (stroke + (width 0) + (type default) + ) + (uuid "e95e6a1a-6ffa-4e61-b05c-fa534f81f2cf") + ) (wire (pts (xy 26.67 34.29) (xy 24.13 34.29) @@ -11510,6 +11540,16 @@ ) (uuid "cee2f43a-7d22-4585-a857-73949bd17a9d") ) + (text "Bodge Wire added for PPM" + (exclude_from_sim no) + (at 212.852 33.782 0) + (effects + (font + (size 1.27 1.27) + ) + ) + (uuid "d8eede4c-b42c-44f8-a357-19b17939766b") + ) (text "black" (exclude_from_sim no) (at 60.325 99.06 0) @@ -13980,7 +14020,7 @@ ) (symbol (lib_id "power:GNDD") - (at 209.55 33.655 0) + (at 198.12 35.56 0) (unit 1) (exclude_from_sim no) (in_bom yes) @@ -13989,7 +14029,7 @@ (fields_autoplaced yes) (uuid "073f2160-76bb-422b-a642-f8ccef9d914c") (property "Reference" "#PWR0111" - (at 209.55 40.005 0) + (at 198.12 41.91 0) (effects (font (size 1.27 1.27) @@ -13998,7 +14038,7 @@ ) ) (property "Value" "GNDD" - (at 209.55 38.1 0) + (at 198.12 40.005 0) (effects (font (size 1.27 1.27) @@ -14006,7 +14046,7 @@ ) ) (property "Footprint" "" - (at 209.55 33.655 0) + (at 198.12 35.56 0) (effects (font (size 1.27 1.27) @@ -14015,7 +14055,7 @@ ) ) (property "Datasheet" "" - (at 209.55 33.655 0) + (at 198.12 35.56 0) (effects (font (size 1.27 1.27) @@ -14024,7 +14064,7 @@ ) ) (property "Description" "" - (at 209.55 33.655 0) + (at 198.12 35.56 0) (effects (font (size 1.27 1.27) diff --git a/controller_teensy/include/definitions.h b/controller_teensy/include/definitions.h index 7119f19..cfea086 100644 --- a/controller_teensy/include/definitions.h +++ b/controller_teensy/include/definitions.h @@ -95,10 +95,11 @@ bool error_brake_outofrange=false; bool error_ads_max_read_interval=false; bool error_sdfile_unavailable=false; -#define REVERSE_ENABLE_TIME 500 //ms. how long standstill to be able to drive backward +#define REVERSE_ENABLE_TIME 300 //ms. how long standstill to be able to drive backward #define NORMAL_MAX_ACCELERATION_RATE 10000 +#define MEDIUM_MAX_ACCELERATION_RATE 500 #define SLOW_MAX_ACCELERATION_RATE 250 int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send increase per second diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h index 80e93bd..7c9565d 100644 --- a/controller_teensy/include/display.h +++ b/controller_teensy/include/display.h @@ -16,10 +16,12 @@ Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); uint8_t standingDisplayScreen=0; -#define NUM_STANDINGDISPLAYSCREEN 4 +#define NUM_STANDINGDISPLAYSCREEN 5 #define DISPLAYSTANDSTILLTIME 5000 +#include "rc.h" + bool display_init(); void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear); void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear); @@ -311,7 +313,7 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc switch(standingDisplayScreen) { - case 0: + case 0: //Quick overview of inputs and status //Row1 @@ -405,7 +407,7 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc break; - case 3: + case 3: //Raw inputs and Input Debug //TODO: show deviation (and max deviation), show resulting throttle and brake pos @@ -442,6 +444,42 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc break; + + case 4: + //Row 1 + display.print(F("RC ")); + dtostrf(ppm[0],4,0,buf); + display.print((String)buf); + display.print(F("/")); + dtostrf(ppm[1],4,0,buf); + display.print((String)buf); + display.println(); + + //Row 2 + + display.print(F("T")); + dtostrf(getRCThrottle(),3,0,buf); + display.print((String)buf); + display.print(F(" B")); + dtostrf(getRCBrake(),3,0,buf); + display.print((String)buf); + display.print(F(" S")); + dtostrf(getRCSteer(),3,0,buf); + display.print((String)buf); + + display.println(); + + //Row 3 + display.print(F("Mode=")); + dtostrf(getRCMode(3),1,0,buf); + display.print((String)buf); + display.print(F(" %=")); + dtostrf(getPPMSuccessrate(),1,5,buf); + display.print((String)buf); + display.println(); + + break; + } //Put User Inputs in last row diff --git a/controller_teensy/include/led.h b/controller_teensy/include/led.h index 3dd5eee..37468e1 100644 --- a/controller_teensy/include/led.h +++ b/controller_teensy/include/led.h @@ -31,6 +31,8 @@ unsigned long last_ledupdate=0; uint8_t led_errorcount=0; //count led progress errors. used for delay at end if any errors occured +extern bool rc_enabled; + void led_dotcircle(unsigned long loopmillis); void led_gauge(unsigned long loopmillis,float value,float value_min,float value_max,uint32_t colorGauge,uint32_t colorBG); @@ -109,18 +111,22 @@ void led_update(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& }else{ if (armed) { - if (loopmillis-last_notidle>5000) { - //Standing - float vbat=min(escRear.getFeedback_batVoltage(),escFront.getFeedback_batVoltage()); - led_gauge(loopmillis,vbat,3.3*12,4.2*12,strip.Color(0, 255, 0, 0),strip.Color(100, 0, 0, 0)); - }else{ - //Driving - float currentMean=escRear.getFiltered_curL()+escRear.getFiltered_curR()+escFront.getFiltered_curL()+escFront.getFiltered_curR(); - uint32_t _bgColor=strip.Color(0, 0, 20, 0); - if (reverse_enabled) { - _bgColor=strip.Color(20, 0, 20, 0); + if (rc_enabled) { + led_gauge(loopmillis,escFront.getCmdL(),0,1000,strip.Color(100, 0, 0, 0),strip.Color(0, 50, 50, 0)); + }else{ //normal driving + if (loopmillis-last_notidle>5000) { + //Standing + float vbat=min(escRear.getFeedback_batVoltage(),escFront.getFeedback_batVoltage()); + led_gauge(loopmillis,vbat,3.3*12,4.2*12,strip.Color(0, 255, 0, 0),strip.Color(100, 0, 0, 0)); + }else{ + //Driving + float currentMean=escRear.getFiltered_curL()+escRear.getFiltered_curR()+escFront.getFiltered_curL()+escFront.getFiltered_curR(); + uint32_t _bgColor=strip.Color(0, 0, 20, 0); + if (reverse_enabled) { + _bgColor=strip.Color(20, 0, 20, 0); + } + led_gauge(loopmillis,currentMean,0,32.0,strip.Color(0, 0, 0, 255),_bgColor); } - led_gauge(loopmillis,currentMean,0,32.0,strip.Color(0, 0, 0, 255),_bgColor); } }else{ //Disarmed diff --git a/controller_teensy/include/rc.h b/controller_teensy/include/rc.h new file mode 100644 index 0000000..b255b32 --- /dev/null +++ b/controller_teensy/include/rc.h @@ -0,0 +1,124 @@ +#ifndef _RC_H_ +#define _RC_H_ + + +#define RC + +#include +PulsePositionInput ppmIn; + +#define PPMREADPERIOD 20 //period for d4r-ii is 18ms +static int count=0; + +#define PIN_PPM 9 + +#define PPM_CHANNELS 8 +#define PPM_SAVE_MIN 950 //minimum allowed for valid data +#define PPM_SAVE_MAX 2050 //maximum allowed for valid data +#define PPM_MIN 1000 +#define PPM_MAX 2000 +#define PPM_MID 1500 +#define PPM_DEADBAND 5 +#define PPM_CHANNEL_THROTTLE 1 +#define PPM_CHANNEL_STEER 2 +#define PPM_CHANNEL_MODE 5 +float ppm[PPM_CHANNELS]; + +unsigned long ppm_count_success=0; +unsigned long ppm_count_fail=0; + +void initRC(); +void readPPM(unsigned long loopmillis); +void printPPM(); + +int16_t getRCThrottle(); +int16_t getRCSteer(); +uint8_t getRCMode(uint8_t modes); + +bool rc_enabled=false; + +void initRC() { + ppmIn.begin(PIN_PPM); +} + +void readPPM(unsigned long loopmillis) { + static unsigned long ppm_last_received=0; + static unsigned long ppm_last_received_success=0; + int ppm_num = ppmIn.available(); + if (ppm_num > 0) { + bool ppm_ok=true; + ppm_last_received=loopmillis; + float new_ppm[PPM_CHANNELS]; + if (ppm_num != PPM_CHANNELS) { + ppm_ok=false; + }else{ + for (int i=1; i <= ppm_num; i++) { + float val=ppmIn.read(i); + new_ppm[i-1]=val; + if (valPPM_SAVE_MAX) { + ppm_ok=false; + } + } + } + + if (ppm_ok) { + ppm_count_success+=1; + ppm_last_received_success=loopmillis; + std::copy(std::begin(new_ppm), std::end(new_ppm), std::begin(ppm)); + }else{ + ppm_count_fail+=1; + } + } +} + +float getPPMSuccessrate() { + float rate=ppm_count_success*1.0/(ppm_count_success+ppm_count_fail); + if (ppm_count_success+ppm_count_fail>500) { + ppm_count_success=0; + ppm_count_fail=0; + } + return rate; +} + +//Return value 0 - 1000 +int16_t getRCThrottle() { + return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000); +} + +int16_t getRCBrake() { + return constrain(map(ppm[PPM_CHANNEL_THROTTLE-1],PPM_MID-PPM_DEADBAND,PPM_MIN,0,1000), 0,1000); +} + +//Return value -1000 - 1000 +int16_t getRCSteer() { + if (ppm[PPM_CHANNEL_STEER-1]>PPM_MID+PPM_DEADBAND){ + return constrain(map(ppm[PPM_CHANNEL_STEER-1],PPM_MID+PPM_DEADBAND,PPM_MAX,0,1000), 0,1000); + }else if (ppm[PPM_CHANNEL_STEER-1] PPMREADPERIOD) { //read digital input states + last_ppmread=loopmillis; + readPPM(loopmillis); + } + failChecks(); @@ -398,14 +411,7 @@ void readADC() { - if (throttle_pos>0 || ((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0) >0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving) - last_notidle=loopmillis; - reverse_enabled=false; - } - if (loopmillis-last_notidle > REVERSE_ENABLE_TIME) { - reverse_enabled=true; - } int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake throttle_pos=constrain(throttlebreak_pos,0,1000); @@ -503,9 +509,39 @@ void failChecks() { void calculateSetSpeed(unsigned long timediff){ int16_t adjusted_throttle_pos=constrain(throttle_pos*(throttle_max/1000.0),0,throttle_max); + int16_t brake_pos_linear=brake_pos; + int16_t brake_pos_expo = (int16_t)(pow((brake_pos_linear/1000.0),2)*1000); - int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000); + +#ifdef RC + if (throttle_pos>0 || brake_pos>0) { //touch any pedal to cancel rc mode + rc_enabled=false; + } + + if (rc_enabled) { + int16_t rcthrottle_pos=getRCThrottle(); + int16_t rcbrake_pos=getRCBrake(); + + + adjusted_throttle_pos=constrain(rcthrottle_pos*(throttle_max/1000.0),0,throttle_max); //overwrite with rc values + brake_pos_linear=rcbrake_pos; + brake_pos_expo = (int16_t)(pow((brake_pos_linear/1000.0),2)*1000); + } + +#endif + + //Reverse Enable + if (throttle_pos>0 || ((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0) >0.5 || (!reverse_enabled && brake_pos_linear>0)) { //reset idle time on these conditions (disables reverse driving) + last_notidle=loopmillis; + reverse_enabled=false; + } + if (loopmillis-last_notidle > REVERSE_ENABLE_TIME) { + reverse_enabled=true; + } + + + //Throttle Management float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000) int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*timediff/1000)); //reduce cmd value every cycle @@ -538,7 +574,7 @@ void calculateSetSpeed(unsigned long timediff){ int16_t cmd_send_toMotor_FR=0; int16_t cmd_send_toMotor_RL=0; int16_t cmd_send_toMotor_RR=0; - int16_t _cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,throttle_max); //brake "ducking" + int16_t _cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos_expo*0.5/1000.0) ) ,0,throttle_max); //brake "ducking" cmd_send_toMotor_FL=_cmd_send_toMotor; @@ -546,6 +582,17 @@ void calculateSetSpeed(unsigned long timediff){ cmd_send_toMotor_RL=_cmd_send_toMotor; cmd_send_toMotor_RR=_cmd_send_toMotor; +#ifdef RC + int16_t differential_steering_max=100; + + if (rc_enabled) { + float rcsteer=getRCSteer()/1000.0; + cmd_send_toMotor_FL+=rcsteer*differential_steering_max; + cmd_send_toMotor_FR-=rcsteer*differential_steering_max; + } + +#endif + #ifdef DRIVING_TANKSTEERING @@ -589,7 +636,7 @@ void calculateSetSpeed(unsigned long timediff){ // ## Braking, Reversing and Standstill movements below here ## /* if (reverse_enabled) { //backwards driving not prohibited - _cmd_send_toMotor-=brake_pos*reverse_speed; + _cmd_send_toMotor-=brake_pos_linear*reverse_speed; cmd_send_toMotor_FL=_cmd_send_toMotor; cmd_send_toMotor_FR=_cmd_send_toMotor; @@ -600,7 +647,7 @@ void calculateSetSpeed(unsigned long timediff){ */ - float steeringdifferential_speed=0.3; //Speed for turning the steering wheel by differtially driving the front wheels + float steeringdifferential_speed=0.4; //Speed for turning the steering wheel by differtially driving the front wheels if (reverse_enabled) { //backwards driving not prohibited static bool tanksteering_prohibit_left=false; @@ -616,7 +663,7 @@ void calculateSetSpeed(unsigned long timediff){ }else if(!control_buttonA && !control_buttonB){ //no button pressed tanksteering_prohibit_left=true; tanksteering_prohibit_right=true; - if (throttle_pos<=0 && brake_pos<=0) {//levers released + if (throttle_pos<=0 && brake_pos_linear<=0) {//levers released tanksteering_prohibit_left=false; tanksteering_prohibit_right=false; prohibit_reverse=false; @@ -624,17 +671,17 @@ void calculateSetSpeed(unsigned long timediff){ } if (!tanksteering_prohibit_right && tanksteering_prohibit_left && prohibit_reverse) { //Right Tanksteering - cmd_send_toMotor_FL+=brake_pos*steeringdifferential_speed; - cmd_send_toMotor_FR-=brake_pos*steeringdifferential_speed; - cmd_send_toMotor_RL+=brake_pos*steeringdifferential_speed; - cmd_send_toMotor_RR-=brake_pos*steeringdifferential_speed; + cmd_send_toMotor_FL+=brake_pos_expo*steeringdifferential_speed; + cmd_send_toMotor_FR-=brake_pos_expo*steeringdifferential_speed; + cmd_send_toMotor_RL+=brake_pos_expo*steeringdifferential_speed; + cmd_send_toMotor_RR-=brake_pos_expo*steeringdifferential_speed; }else if(tanksteering_prohibit_right && !tanksteering_prohibit_left && prohibit_reverse) { //Left Tanksteering - cmd_send_toMotor_FL-=brake_pos*steeringdifferential_speed; - cmd_send_toMotor_FR+=brake_pos*steeringdifferential_speed; - cmd_send_toMotor_RL-=brake_pos*steeringdifferential_speed; - cmd_send_toMotor_RR+=brake_pos*steeringdifferential_speed; + cmd_send_toMotor_FL-=brake_pos_expo*steeringdifferential_speed; + cmd_send_toMotor_FR+=brake_pos_expo*steeringdifferential_speed; + cmd_send_toMotor_RL-=brake_pos_expo*steeringdifferential_speed; + cmd_send_toMotor_RR+=brake_pos_expo*steeringdifferential_speed; }else if(tanksteering_prohibit_right && tanksteering_prohibit_left && !prohibit_reverse){ //no button on steering wheel pressed, drive backwards - _cmd_send_toMotor-=brake_pos*reverse_speed; + _cmd_send_toMotor-=brake_pos_expo*reverse_speed; cmd_send_toMotor_FL=_cmd_send_toMotor; cmd_send_toMotor_FR=_cmd_send_toMotor; @@ -716,18 +763,28 @@ void readButtons() { if (escFront.getControllerConnected() && escRear.getControllerConnected()) { armed=true; //arm if button pressed long enough writeLogComment(loopmillis, "Armed by button"); - if (control_buttonA) { //button A is held down during start button press + rc_enabled=false; + if (control_buttonA && control_buttonB) { //button A and B is held down during start button press + throttle_max=250; + reverse_speed=0.15; + max_acceleration_rate=MEDIUM_MAX_ACCELERATION_RATE; + rc_enabled=true; + writeLogComment(loopmillis, "Mode: RC"); + }else if (control_buttonA) { //button A is held down during start button press throttle_max=1000; reverse_speed=0.25; max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: Fast"); }else if (control_buttonB) { //button B is held down during start button press throttle_max=750; reverse_speed=0.25; max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: Medium"); }else { //no control button pressed during start throttle_max=250; reverse_speed=0.15; max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: Slow"); } }else{