From 681e33b355fd44671f3119eef00e0ad4be5fb9ff Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 22 Sep 2024 19:03:46 +0200 Subject: [PATCH] optimize rc control --- Bobbycar_wiring/Bobbycar_wiring.kicad_pro | 4 +- Bobbycar_wiring/Bobbycar_wiring.kicad_sch | 40 ------ .../.vscode/c_cpp_properties.json | 2 + controller_teensy/.vscode/settings.json | 47 ++++++- controller_teensy/include/display.h | 13 +- controller_teensy/include/led.h | 5 +- controller_teensy/include/rc.h | 7 +- controller_teensy/src/main.cpp | 115 ++++++++++++++---- logdata_visualization/analyze.py | 103 +++++++++++++--- 9 files changed, 241 insertions(+), 95 deletions(-) diff --git a/Bobbycar_wiring/Bobbycar_wiring.kicad_pro b/Bobbycar_wiring/Bobbycar_wiring.kicad_pro index a5257c5..ba947f2 100644 --- a/Bobbycar_wiring/Bobbycar_wiring.kicad_pro +++ b/Bobbycar_wiring/Bobbycar_wiring.kicad_pro @@ -59,7 +59,9 @@ "width": 0.0 } ], - "drc_exclusions": [], + "drc_exclusions": [ + "lib_footprint_mismatch|93919000|108539000|df2709a9-a255-4f53-9496-85732562ad08|00000000-0000-0000-0000-000000000000" + ], "meta": { "version": 2 }, diff --git a/Bobbycar_wiring/Bobbycar_wiring.kicad_sch b/Bobbycar_wiring/Bobbycar_wiring.kicad_sch index 0a3c11a..ea0e4bb 100644 --- a/Bobbycar_wiring/Bobbycar_wiring.kicad_sch +++ b/Bobbycar_wiring/Bobbycar_wiring.kicad_sch @@ -10551,16 +10551,6 @@ ) (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) @@ -10831,16 +10821,6 @@ ) (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) @@ -11011,16 +10991,6 @@ ) (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) @@ -11540,16 +11510,6 @@ ) (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) diff --git a/controller_teensy/.vscode/c_cpp_properties.json b/controller_teensy/.vscode/c_cpp_properties.json index f449eaa..88f283d 100644 --- a/controller_teensy/.vscode/c_cpp_properties.json +++ b/controller_teensy/.vscode/c_cpp_properties.json @@ -14,6 +14,7 @@ "/home/fisch/.platformio/packages/framework-arduinoteensy/libraries/SdFat/src", "/home/fisch/.platformio/packages/framework-arduinoteensy/libraries/Time", "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/lib/hoverboard-esc-serial-comm/src", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/.pio/libdeps/teensy41/PulsePosition", "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/.pio/libdeps/teensy41/DallasTemperature", "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/.pio/libdeps/teensy41/OneWire", "/home/fisch/.platformio/packages/framework-arduinoteensy/libraries/VirtualWire", @@ -129,6 +130,7 @@ "/home/fisch/.platformio/packages/framework-arduinoteensy/libraries/SdFat/src", "/home/fisch/.platformio/packages/framework-arduinoteensy/libraries/Time", "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/lib/hoverboard-esc-serial-comm/src", + "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/.pio/libdeps/teensy41/PulsePosition", "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/.pio/libdeps/teensy41/DallasTemperature", "/media/fisch/HDD/Projects/bobbycar/bobbycar_repo/controller_teensy/.pio/libdeps/teensy41/OneWire", "/home/fisch/.platformio/packages/framework-arduinoteensy/libraries/VirtualWire", diff --git a/controller_teensy/.vscode/settings.json b/controller_teensy/.vscode/settings.json index cad7657..1bfdb81 100644 --- a/controller_teensy/.vscode/settings.json +++ b/controller_teensy/.vscode/settings.json @@ -1,3 +1,48 @@ { - "cmake.configureOnOpen": false + "cmake.configureOnOpen": false, + "files.associations": { + "algorithm": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "concepts": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "random": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "initializer_list": "cpp", + "iosfwd": "cpp", + "limits": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "cinttypes": "cpp", + "typeinfo": "cpp" + } } \ No newline at end of file diff --git a/controller_teensy/include/display.h b/controller_teensy/include/display.h index 7c9565d..2643dde 100644 --- a/controller_teensy/include/display.h +++ b/controller_teensy/include/display.h @@ -137,6 +137,17 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { display.setCursor(SCREEN_WIDTH-25,SCREEN_HEIGHT-16); display.print("km/h"); +#ifdef RC + display.setCursor(0,0); + if (rc_fullcontrol_enabled) { + display.print("RC Fullcontrol"); + }else if(rc_steer_enabled){ + display.print("RC Steering"); + } + + +#endif + //A display.setCursor(SCREEN_WIDTH-37,1); static float averaged_filtered_currentAll; @@ -471,7 +482,7 @@ void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& esc //Row 3 display.print(F("Mode=")); - dtostrf(getRCMode(3),1,0,buf); + dtostrf(getRCMode(6),1,0,buf); display.print((String)buf); display.print(F(" %=")); dtostrf(getPPMSuccessrate(),1,5,buf); diff --git a/controller_teensy/include/led.h b/controller_teensy/include/led.h index 37468e1..50fcdb2 100644 --- a/controller_teensy/include/led.h +++ b/controller_teensy/include/led.h @@ -31,7 +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; +extern bool rc_fullcontrol_enabled; //from rc.h +extern bool rc_steer_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); @@ -111,7 +112,7 @@ void led_update(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& }else{ if (armed) { - if (rc_enabled) { + if (rc_fullcontrol_enabled || rc_steer_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) { diff --git a/controller_teensy/include/rc.h b/controller_teensy/include/rc.h index b255b32..1c3ee20 100644 --- a/controller_teensy/include/rc.h +++ b/controller_teensy/include/rc.h @@ -35,19 +35,17 @@ int16_t getRCThrottle(); int16_t getRCSteer(); uint8_t getRCMode(uint8_t modes); -bool rc_enabled=false; +bool rc_fullcontrol_enabled=false; //full remote control. can be cancelled by button press +bool rc_steer_enabled=false; //only rc steering 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; @@ -63,7 +61,6 @@ void readPPM(unsigned long loopmillis) { 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; diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index 33b8d66..6b7abee 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -515,11 +515,11 @@ void calculateSetSpeed(unsigned long timediff){ #ifdef RC - if (throttle_pos>0 || brake_pos>0) { //touch any pedal to cancel rc mode - rc_enabled=false; + if (throttle_pos>0 || brake_pos>0) { //touch any pedal to cancel rc fullcontrol mode + rc_fullcontrol_enabled=false; } - if (rc_enabled) { + if (rc_fullcontrol_enabled) { int16_t rcthrottle_pos=getRCThrottle(); int16_t rcbrake_pos=getRCBrake(); @@ -583,12 +583,24 @@ void calculateSetSpeed(unsigned long timediff){ cmd_send_toMotor_RR=_cmd_send_toMotor; #ifdef RC - int16_t differential_steering_max=100; + int16_t differential_steering_max=200; - if (rc_enabled) { - float rcsteer=getRCSteer()/1000.0; - cmd_send_toMotor_FL+=rcsteer*differential_steering_max; - cmd_send_toMotor_FR-=rcsteer*differential_steering_max; + #define RC_STEER_ENABLE_WAITTIME 1000/SENDPERIOD*2; //in cycles. every SENDPERIOD + static uint16_t rc_steer_enable_wait=0; //give some time to release buttons after arming with rc mode to not cancel immediately + if (rc_steer_enabled) { + if (rc_steer_enable_wait>0) { + rc_steer_enable_wait--; + }else{ //allow control when counter reached 0 + if (control_buttonA || control_buttonB) { //cancel rc modes by button press + rc_fullcontrol_enabled=false; + rc_steer_enabled=false; + } + float rcsteer=getRCSteer()/1000.0; + cmd_send_toMotor_FL+=rcsteer*differential_steering_max; + cmd_send_toMotor_FR-=rcsteer*differential_steering_max; + } + }else{ + rc_steer_enable_wait=RC_STEER_ENABLE_WAITTIME; } #endif @@ -671,22 +683,22 @@ void calculateSetSpeed(unsigned long timediff){ } if (!tanksteering_prohibit_right && tanksteering_prohibit_left && prohibit_reverse) { //Right Tanksteering - 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; + cmd_send_toMotor_FL+=brake_pos_linear*steeringdifferential_speed; + cmd_send_toMotor_FR-=brake_pos_linear*steeringdifferential_speed; + cmd_send_toMotor_RL+=brake_pos_linear*steeringdifferential_speed; + cmd_send_toMotor_RR-=brake_pos_linear*steeringdifferential_speed; }else if(tanksteering_prohibit_right && !tanksteering_prohibit_left && prohibit_reverse) { //Left Tanksteering - 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; + cmd_send_toMotor_FL-=brake_pos_linear*steeringdifferential_speed; + cmd_send_toMotor_FR+=brake_pos_linear*steeringdifferential_speed; + cmd_send_toMotor_RL-=brake_pos_linear*steeringdifferential_speed; + cmd_send_toMotor_RR+=brake_pos_linear*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_expo*reverse_speed; + int16_t cmd_send_brake=brake_pos_linear*reverse_speed; - cmd_send_toMotor_FL=_cmd_send_toMotor; - cmd_send_toMotor_FR=_cmd_send_toMotor; - cmd_send_toMotor_RL=_cmd_send_toMotor; - cmd_send_toMotor_RR=_cmd_send_toMotor; + cmd_send_toMotor_FL-=cmd_send_brake; + cmd_send_toMotor_FR-=cmd_send_brake; + cmd_send_toMotor_RL-=cmd_send_brake; + cmd_send_toMotor_RR-=cmd_send_brake; } } @@ -763,13 +775,62 @@ void readButtons() { if (escFront.getControllerConnected() && escRear.getControllerConnected()) { armed=true; //arm if button pressed long enough writeLogComment(loopmillis, "Armed by button"); - rc_enabled=false; + rc_fullcontrol_enabled=false; + rc_steer_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"); + uint8_t rcmode=getRCMode(6); + if (getRCThrottle()==0 && getRCBrake()==0) { + switch (rcmode) { + case 0: + throttle_max=250; + reverse_speed=0.15; + max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: RC 0"); + break; + + case 1: + throttle_max=250; + reverse_speed=0.15; + max_acceleration_rate=MEDIUM_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: RC 1"); + break; + + case 2: + throttle_max=300; + reverse_speed=0.2; + max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: RC 2"); + break; + + case 3: + throttle_max=400; + reverse_speed=0.25; + max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: RC 3"); + break; + + case 4: + throttle_max=600; + reverse_speed=0.25; + max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: RC 4"); + break; + + case 5: + throttle_max=750; + reverse_speed=0.25; + max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; + writeLogComment(loopmillis, "Mode: RC 5"); + break; + } + + rc_fullcontrol_enabled=true; + rc_steer_enabled=true; + }else{ + writeLogComment(loopmillis, "Not Arming. RC throttle and brake not zero"); + armed=false; + } + }else if (control_buttonA) { //button A is held down during start button press throttle_max=1000; reverse_speed=0.25; diff --git a/logdata_visualization/analyze.py b/logdata_visualization/analyze.py index 7f58f55..9df7b60 100644 --- a/logdata_visualization/analyze.py +++ b/logdata_visualization/analyze.py @@ -7,6 +7,13 @@ import numpy as np import argparse +def mapRange(value, inMin, inMax, outMin, outMax,constrain=False): + if constrain: + return max(outMin, min(outMax, outMin + (((value - inMin) / (inMax - inMin)) * (outMax - outMin)))) + else: + return outMin + (((value - inMin) / (inMax - inMin)) * (outMax - outMin)) + + parser = argparse.ArgumentParser(description='Analyzes fixed csv logs from bobbycar') parser.add_argument('-i', '--input', type=argparse.FileType('r'), required=True, help="input csv log file") args = parser.parse_args() @@ -17,31 +24,91 @@ x = df['timestamp'] x = [i-x[0] for i in x] #offset time by starttime -scattersize=1 -scatteralpha=0.1 + +def template(): + scattersize=1 + scatteralpha=0.1 -fig, ax1 = plt.subplots() + fig, ax1 = plt.subplots() -ax2 = ax1.twinx() + ax2 = ax1.twinx() -#plt.scatter(x,df['rpm_FrontL'], s=scattersize, alpha=scatteralpha, label="rpm_FrontL") -ax1.plot(x,np.array(df['vbat_Front']), c='b', alpha=0.5, label="vbat_Front") -ax1.plot(x,np.array(df['vbat_Rear']), c='r', alpha=0.5, label="vbat_Rear") -ax2.plot(x,np.array(df['rpm_FrontL']), c='g', alpha=0.5, label="rpm_FrontL") -#plt.plot(x,np.array(df['currentConsumed']), c='g', alpha=0.5, label="currentConsumed") + #plt.scatter(x,df['rpm_FrontL'], s=scattersize, alpha=scatteralpha, label="rpm_FrontL") + ax1.plot(x,np.array(df['temp_Front']), c='#0000ff', alpha=0.5, label="temp_Front") + ax1.plot(x,np.array(df['temp_Rear']), c='#ff0000', alpha=0.5, label="temp_Rear") + ax1.plot(x,np.array(df['temp_Air']), c='#00ff00', alpha=0.5, label="temp_Air") + #ax2.plot(x,np.array(df['throttle']), c='g', alpha=0.5, label="throttle") -#plt.scatter(x,df['rpm_FrontR'], s=scattersize, alpha=scatteralpha, label="rpm_FrontR") -ax1.set_xlabel('timestamp') -#plt.ylabel('data') -ax1.set_ylabel('first axis') -ax2.set_ylabel('second axis') -#plt.title('') -ax1.legend(loc='upper left') -ax2.legend(loc='upper right') -plt.show() + #confidence + #confidence_FrontL=[abs(x-df['cmd_FrontL'][i-5 if i>4 else 0])+abs(x-df['cmd_FrontL'][i-10 if i>9 else 0])+abs(x-df['cmd_FrontL'][i-20 if i>19 else 0]) for i,x in enumerate(df['cmd_FrontL'])] + #confidence_FrontL=[mapRange(x,0,50,1.0,0.0,True) for x in confidence_FrontL] + + + #ax1.scatter(df['cmd_FrontL'],df['rpm_FrontL'], s=scattersize, alpha=scatteralpha, label="FrontL") + #ax1.scatter(df['cmd_FrontR'],df['rpm_FrontR'], s=scattersize, alpha=scatteralpha, label="FrontR") + #ax1.scatter(df['cmd_RearL'],df['rpm_RearL'], s=scattersize, alpha=scatteralpha, label="RearL") + #ax1.scatter(df['cmd_RearR'],df['rpm_RearR'], s=scattersize, alpha=scatteralpha, label="RearR") + + ax1.set_xlabel('timestamp') + #plt.ylabel('data') + ax1.set_ylabel('first axis') + ax2.set_ylabel('second axis') + #plt.title('') + ax1.legend(loc='upper left') + ax2.legend(loc='upper right') + plt.show() + + +def rpmDifference(): + scattersize=1 + scatteralpha=0.1 + + + fig, ax1 = plt.subplots() + + #confidence_FrontL=[abs(x-df['cmd_FrontL'][i-5 if i>4 else 0])+abs(x-df['cmd_FrontL'][i-10 if i>9 else 0])+abs(x-df['cmd_FrontL'][i-20 if i>19 else 0]) for i,x in enumerate(df['cmd_FrontL'])] + frontRPMdiff=np.convolve(np.array(df['rpm_FrontL'])-np.array(df['rpm_FrontR']), np.ones(10)/10, mode='same') + rearRPMdiff=np.convolve(np.array(df['rpm_RearL'])-np.array(df['rpm_RearR']), np.ones(10)/10, mode='same') + + ax1.plot(x,frontRPMdiff, c='#0000ff', alpha=0.5, label="rpm Difference Front") + ax1.plot(x,rearRPMdiff, c='#00ff00', alpha=0.5, label="rpm Difference Rear") + + ax1.set_xlabel('timestamp') + ax1.set_ylabel('rpm Difference') + ax1.legend(loc='upper left') + plt.show() + + +def plot_rpmVsCurrent(): + scattersize=2 + scatteralpha=0.5 + + #confidence + #confidence_FrontL=[abs(x-df['cmd_FrontL'][i-5 if i>4 else 0])+abs(x-df['cmd_FrontL'][i-10 if i>9 else 0])+abs(x-df['cmd_FrontL'][i-20 if i>19 else 0]) for i,x in enumerate(df['cmd_FrontL'])] + #confidence_FrontL=[mapRange(x,0,50,1.0,0.0,True) for x in confidence_FrontL] + + + + fig, ax1 = plt.subplots() + ax1.scatter(df['rpm_FrontL'],df['current_FrontL'], s=5, alpha=scatteralpha, label="FrontL") + ax1.scatter(df['rpm_FrontR'],df['current_FrontR'], s=5, alpha=scatteralpha, label="FrontR") + #ax1.scatter(df['cmd_FrontR'],df['rpm_FrontR'], s=scattersize, alpha=scatteralpha, label="FrontR") + #ax1.scatter(df['cmd_RearL'],df['rpm_RearL'], s=scattersize, alpha=scatteralpha, label="RearL") + #ax1.scatter(df['cmd_RearR'],df['rpm_RearR'], s=scattersize, alpha=scatteralpha, label="RearR") + + ax1.set_xlabel('rpm') + ax1.set_ylabel('current (A)') + plt.title('rpm vs current') + ax1.legend(loc='upper left') + plt.show() + + +#plot_rpmVsCurrent() +rpmDifference() + exit()