optimize rc control

This commit is contained in:
interfisch 2024-09-22 19:03:46 +02:00
parent 67f0a2cf7e
commit 681e33b355
9 changed files with 241 additions and 95 deletions

View File

@ -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
},

View File

@ -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)

View File

@ -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",

View File

@ -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"
}
}

View File

@ -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);

View File

@ -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) {

View File

@ -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;

View File

@ -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;

View File

@ -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()