Skip to content

C
car-controlserver
Commit d49ec850 authored by 957dd's avatar 957dd
Browse files
Options

修改了0107的运动相关代码

parent 80c9d008
Show whitespace changes
Inline Side-by-side
Showing with 163 additions and 19 deletions
build/main
View file @ d49ec850
No preview for this file type
drivers/devicecontrol/car/car0101_control.c
View file @ d49ec850
...@@ -34,6 +34,9 @@ void car0101_mode_1_flont(int gval) { ...@@ -34,6 +34,9 @@ void car0101_mode_1_flont(int gval) {
pwmWrite(PWM_PIN_SPEED, 70-b+a); pwmWrite(PWM_PIN_SPEED, 70-b+a);
} else if(gval >70){ } else if(gval >70){
int back_speed = 70 - (gval-70)/10-b+a; int back_speed = 70 - (gval-70)/10-b+a;
if (back_speed < 55) {
back_speed = 55;
}
pwmWrite(PWM_PIN_SPEED, back_speed); pwmWrite(PWM_PIN_SPEED, back_speed);
} }
} }
...@@ -49,6 +52,9 @@ void car0101_mode_2_back(int gval) { ...@@ -49,6 +52,9 @@ void car0101_mode_2_back(int gval) {
pwmWrite(PWM_PIN_SPEED, 80+b-a); pwmWrite(PWM_PIN_SPEED, 80+b-a);
}else if(gval >70){ }else if(gval >70){
int flont_speed = 80+(gval-70)/10+b-a; int flont_speed = 80+(gval-70)/10+b-a;
if (flont_speed > 95) {
flont_speed = 95;
}
pwmWrite(PWM_PIN_SPEED, flont_speed); pwmWrite(PWM_PIN_SPEED, flont_speed);
} }
} }
......
drivers/devicecontrol/car/car0103_control.c
View file @ d49ec850
...@@ -19,6 +19,9 @@ void mode_car0103_lift_flont(int gval) { ...@@ -19,6 +19,9 @@ void mode_car0103_lift_flont(int gval) {
pwmWrite(PWM_PIN_SPEED, 80+b); pwmWrite(PWM_PIN_SPEED, 80+b);
}else if(gval >70){ }else if(gval >70){
int flont_speed = 80+(gval-70)/10+b; int flont_speed = 80+(gval-70)/10+b;
if (flont_speed > 95) {
flont_speed = 95;
}
pwmWrite(PWM_PIN_SPEED, flont_speed); pwmWrite(PWM_PIN_SPEED, flont_speed);
} }
} }
...@@ -33,6 +36,9 @@ void mode_car0103_lift_back(int gval) { ...@@ -33,6 +36,9 @@ void mode_car0103_lift_back(int gval) {
pwmWrite(PWM_PIN_SPEED, 70-b); pwmWrite(PWM_PIN_SPEED, 70-b);
} else if(gval >70){ } else if(gval >70){
int back_speed = 70 - (gval-70)/10-b; int back_speed = 70 - (gval-70)/10-b;
if (back_speed < 55) {
back_speed = 55;
}
pwmWrite(PWM_PIN_SPEED, back_speed); pwmWrite(PWM_PIN_SPEED, back_speed);
} }
} }
...@@ -47,6 +53,9 @@ void mode_car0103_right_flont(int gval) { ...@@ -47,6 +53,9 @@ void mode_car0103_right_flont(int gval) {
pwmWrite(PWM_PIN_CHANGE, 80+b); pwmWrite(PWM_PIN_CHANGE, 80+b);
}else if(gval >70){ }else if(gval >70){
int flont_speed = 80+(gval-70)/10+b; int flont_speed = 80+(gval-70)/10+b;
if (flont_speed > 95) {
flont_speed = 95;
}
pwmWrite(PWM_PIN_CHANGE, flont_speed); pwmWrite(PWM_PIN_CHANGE, flont_speed);
} }
} }
...@@ -61,6 +70,9 @@ void mode_car0103_right_back(int gval) { ...@@ -61,6 +70,9 @@ void mode_car0103_right_back(int gval) {
pwmWrite(PWM_PIN_CHANGE, 70-b); pwmWrite(PWM_PIN_CHANGE, 70-b);
} else if(gval >70){ } else if(gval >70){
int back_speed = 70 - (gval-70)/10-b; int back_speed = 70 - (gval-70)/10-b;
if (back_speed < 55) {
back_speed = 55;
}
pwmWrite(PWM_PIN_CHANGE, back_speed); pwmWrite(PWM_PIN_CHANGE, back_speed);
} }
} }
......
drivers/devicecontrol/car/car0107_control.c
View file @ d49ec850
...@@ -21,13 +21,54 @@ static int s_current_speed = 0; ...@@ -21,13 +21,54 @@ static int s_current_speed = 0;
static int s_speed_mode = 0; static int s_speed_mode = 0;
static int s_speed_timeout = 0; static int s_speed_timeout = 0;
static int s_phone_speed_mode = 0;
static int s_phone_current_gval = 0;
static int s_phone_pending_mode = 0;
static int s_phone_pending_gval = 0;
#define STEER_STEP_TURN 60 #define STEER_STEP_TURN 60
#define STEER_STEP_RETURN 90 #define STEER_STEP_RETURN 90
#define REV_TARGET_SCALE_NUM 6 #define REV_TARGET_SCALE_NUM 6
#define REV_TARGET_SCALE_DEN 10 #define REV_TARGET_SCALE_DEN 10
#define ACCEL_STEP 5 #define ACCEL_STEP 5
#define DECEL_STEP 6 #define DECEL_STEP 6
#define STEER_GVAL_MAX 150 #define BRAKE_STEP 10
#define CAR0107_MAX_INPUT_SPEED 200
#define CAR0107_ACTIVE_THRESHOLD 50
#define CAR0107_MAX_TARGET_BACK 100
#define CAR0107_MAX_TARGET_FRONT 80
#define STEER_GVAL_MAX 120
#define CAR0107_PWM_FORWARD_MIN 58
#define CAR0107_PWM_BACK_MAX 84
static int car0107_clamp_input(int val) {
if (val < 0) {
return 0;
}
if (val > CAR0107_MAX_INPUT_SPEED) {
return CAR0107_MAX_INPUT_SPEED;
}
return val;
}
static int car0107_input_to_gval(int val) {
val = car0107_clamp_input(val);
if (val <= CAR0107_ACTIVE_THRESHOLD) {
return val;
}
return CAR0107_ACTIVE_THRESHOLD +
(val - CAR0107_ACTIVE_THRESHOLD) *
(STEER_GVAL_MAX - CAR0107_ACTIVE_THRESHOLD) /
(CAR0107_MAX_INPUT_SPEED - CAR0107_ACTIVE_THRESHOLD);
}
static int car0107_input_to_target_speed(int val, int max_target) {
val = car0107_clamp_input(val);
if (val <= CAR0107_ACTIVE_THRESHOLD) {
return 0;
}
return (val - 40) * max_target / (CAR0107_MAX_INPUT_SPEED - 40);
}
static void car0107_calculate_L_R(int angle) { static void car0107_calculate_L_R(int angle) {
if (angle < 0) { if (angle < 0) {
...@@ -54,26 +95,34 @@ static void car0107_reset_steering_state(void) { ...@@ -54,26 +95,34 @@ static void car0107_reset_steering_state(void) {
s_steering_timeout = 0; s_steering_timeout = 0;
} }
static void car0107_reset_phone_state(void) {
s_phone_speed_mode = 0;
s_phone_current_gval = 0;
s_phone_pending_mode = 0;
s_phone_pending_gval = 0;
}
static void car0107_apply_stop(void) {
pwmWrite(PWM_PIN_SPEED, 75);
car0107_calculate_L_R(90);
}
static int car0107_speed_to_gval(int speed) { static int car0107_speed_to_gval(int speed) {
if (speed <= 0) { if (speed <= 0) {
return 0; return 0;
} }
int gval = 50 + (speed * 100) / 160; int gval = 50 + (speed * (STEER_GVAL_MAX - 50)) / CAR0107_MAX_TARGET_BACK;
if (gval > STEER_GVAL_MAX) { if (gval > STEER_GVAL_MAX) {
gval = STEER_GVAL_MAX; gval = STEER_GVAL_MAX;
} }
return gval; return gval;
} }
static void car0107_apply_stop(void) {
pwmWrite(PWM_PIN_SPEED, 75);
car0107_calculate_L_R(90);
}
/*车中间值，停止舵机中间函数*/ /*车中间值，停止舵机中间函数*/
void car0107_middle_pwm() { void car0107_middle_pwm() {
pthread_mutex_lock(&s_car0107_hw_mutex); pthread_mutex_lock(&s_car0107_hw_mutex);
car0107_reset_steering_state(); car0107_reset_steering_state();
car0107_reset_phone_state();
car0107_apply_stop(); car0107_apply_stop();
pthread_mutex_unlock(&s_car0107_hw_mutex); pthread_mutex_unlock(&s_car0107_hw_mutex);
} }
...@@ -88,6 +137,9 @@ static void car0107_mode_1_flont(int gval) { ...@@ -88,6 +137,9 @@ static void car0107_mode_1_flont(int gval) {
pwmWrite(PWM_PIN_SPEED, 70-b+a); pwmWrite(PWM_PIN_SPEED, 70-b+a);
} else if(gval >70){ } else if(gval >70){
int back_speed = 70 - (gval-70)/10-b+a; int back_speed = 70 - (gval-70)/10-b+a;
if (back_speed < CAR0107_PWM_FORWARD_MIN) {
back_speed = CAR0107_PWM_FORWARD_MIN;
}
pwmWrite(PWM_PIN_SPEED, back_speed); pwmWrite(PWM_PIN_SPEED, back_speed);
} }
} }
...@@ -103,6 +155,9 @@ static void car0107_mode_2_back(int gval) { ...@@ -103,6 +155,9 @@ static void car0107_mode_2_back(int gval) {
pwmWrite(PWM_PIN_SPEED, 80+b-a); pwmWrite(PWM_PIN_SPEED, 80+b-a);
}else if(gval >70){ }else if(gval >70){
int flont_speed = 80+(gval-70)/10+b-a; int flont_speed = 80+(gval-70)/10+b-a;
if (flont_speed > CAR0107_PWM_BACK_MAX) {
flont_speed = CAR0107_PWM_BACK_MAX;
}
pwmWrite(PWM_PIN_SPEED, flont_speed); pwmWrite(PWM_PIN_SPEED, flont_speed);
} }
} }
...@@ -129,22 +184,63 @@ static void car0107_mode_4_right(int gval) { ...@@ -129,22 +184,63 @@ static void car0107_mode_4_right(int gval) {
} }
} }
static void car0107_phone_apply_speed(void) {
if (s_phone_speed_mode == 1) {
car0107_mode_2_back(s_phone_current_gval);
} else if (s_phone_speed_mode == 2) {
car0107_mode_1_flont(s_phone_current_gval);
} else {
pwmWrite(PWM_PIN_SPEED, 75);
}
}
static void car0107_set_phone_speed_command(int new_mode, int new_gval) {
if (new_gval <= CAR0107_ACTIVE_THRESHOLD) {
car0107_reset_phone_state();
pwmWrite(PWM_PIN_SPEED, 75);
return;
}
if (s_phone_current_gval > CAR0107_ACTIVE_THRESHOLD &&
s_phone_speed_mode != 0 && new_mode != s_phone_speed_mode) {
s_phone_pending_mode = new_mode;
s_phone_pending_gval = new_gval;
s_phone_speed_mode = 0;
pwmWrite(PWM_PIN_SPEED, 75);
return;
}
if (s_phone_speed_mode == 0 && s_phone_pending_mode != 0) {
s_phone_pending_mode = new_mode;
s_phone_pending_gval = new_gval;
return;
}
s_phone_pending_mode = 0;
s_phone_pending_gval = 0;
s_phone_speed_mode = new_mode;
s_phone_current_gval = new_gval;
car0107_phone_apply_speed();
}
void car0107_control_change(int *buf) { void car0107_control_change(int *buf) {
int mode=buf[1]; int mode=buf[1];
int val=buf[2]; int val=car0107_input_to_gval(buf[2]);
pthread_mutex_lock(&s_car0107_hw_mutex); pthread_mutex_lock(&s_car0107_hw_mutex);
s_control_source = CTRL_PHONE; s_control_source = CTRL_PHONE;
s_target_speed = 0;
s_current_speed = 0; s_current_speed = 0;
s_speed_mode = 0;
s_speed_timeout = 0; s_speed_timeout = 0;
s_steering_timeout = 0; s_steering_timeout = 0;
switch(mode){ switch(mode){
case 1: case 1:
car0107_mode_2_back(val); car0107_set_phone_speed_command(1, val);
break; break;
case 2: case 2:
car0107_mode_1_flont(val); car0107_set_phone_speed_command(2, val);
break; break;
case 3: case 3:
car0107_mode_3_left(val); car0107_mode_3_left(val);
...@@ -158,6 +254,36 @@ void car0107_control_change(int *buf) { ...@@ -158,6 +254,36 @@ void car0107_control_change(int *buf) {
pthread_mutex_unlock(&s_car0107_hw_mutex); pthread_mutex_unlock(&s_car0107_hw_mutex);
} }
static void car0107_phone_smooth_process(void) {
if (s_control_source != CTRL_PHONE) {
return;
}
if (s_phone_speed_mode != 0 || s_phone_pending_mode == 0) {
return;
}
if (s_phone_current_gval <= CAR0107_ACTIVE_THRESHOLD) {
s_phone_speed_mode = s_phone_pending_mode;
s_phone_current_gval = s_phone_pending_gval;
s_phone_pending_mode = 0;
s_phone_pending_gval = 0;
car0107_phone_apply_speed();
return;
}
s_phone_current_gval -= BRAKE_STEP;
if (s_phone_current_gval <= CAR0107_ACTIVE_THRESHOLD) {
s_phone_current_gval = 0;
pwmWrite(PWM_PIN_SPEED, 75);
s_phone_speed_mode = s_phone_pending_mode;
s_phone_current_gval = s_phone_pending_gval;
s_phone_pending_mode = 0;
s_phone_pending_gval = 0;
car0107_phone_apply_speed();
} else {
pwmWrite(PWM_PIN_SPEED, 75);
}
}
static void car0107_speed_smooth_process(void) { static void car0107_speed_smooth_process(void) {
if (s_control_source != CTRL_STEERING) { if (s_control_source != CTRL_STEERING) {
return; return;
...@@ -257,6 +383,7 @@ static void car0107_steering_angle_smooth_process(void) { ...@@ -257,6 +383,7 @@ static void car0107_steering_angle_smooth_process(void) {
void car0107_steering_smooth_all(void) { void car0107_steering_smooth_all(void) {
pthread_mutex_lock(&s_car0107_hw_mutex); pthread_mutex_lock(&s_car0107_hw_mutex);
car0107_phone_smooth_process();
car0107_speed_smooth_process(); car0107_speed_smooth_process();
car0107_steering_angle_smooth_process(); car0107_steering_angle_smooth_process();
pthread_mutex_unlock(&s_car0107_hw_mutex); pthread_mutex_unlock(&s_car0107_hw_mutex);
...@@ -268,32 +395,31 @@ void car0107_control_steering_change_process(int *buf) { ...@@ -268,32 +395,31 @@ void car0107_control_steering_change_process(int *buf) {
pthread_mutex_lock(&s_car0107_hw_mutex); pthread_mutex_lock(&s_car0107_hw_mutex);
s_control_source = CTRL_STEERING; s_control_source = CTRL_STEERING;
car0107_reset_phone_state();
s_steering_timeout = 200; s_steering_timeout = 200;
s_speed_timeout = 200; s_speed_timeout = 200;
if (mode == 1) { if (mode == 1) {
s_speed_mode = 1; s_speed_mode = 1;
if (val < 50) { val = car0107_clamp_input(val);
if (val <= CAR0107_ACTIVE_THRESHOLD) {
s_target_speed = 0; s_target_speed = 0;
s_current_speed = 0; s_current_speed = 0;
s_speed_mode = 0; s_speed_mode = 0;
car0107_apply_stop(); car0107_apply_stop();
} else if (val <= 200) {
s_target_speed = val - 40;
} else { } else {
s_target_speed = 160; s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_BACK);
} }
} else if (mode == 2) { } else if (mode == 2) {
s_speed_mode = 2; s_speed_mode = 2;
if (val < 50) { val = car0107_clamp_input(val);
if (val <= CAR0107_ACTIVE_THRESHOLD) {
s_target_speed = 0; s_target_speed = 0;
s_current_speed = 0; s_current_speed = 0;
s_speed_mode = 0; s_speed_mode = 0;
car0107_apply_stop(); car0107_apply_stop();
} else if (val <= 200) {
s_target_speed = (val - 40) * 3 / 4;
} else { } else {
s_target_speed = 80; s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_FRONT);
} }
} else if (mode == 3) { } else if (mode == 3) {
if (val == 0) { if (val == 0) {
......
zlog.conf
View file @ d49ec850
...@@ -9,4 +9,4 @@ file perms = 600 ...@@ -9,4 +9,4 @@ file perms = 600
millisecond = "%d(%Y-%m-%d %H:%M:%S).%ms [%V] %m%n" millisecond = "%d(%Y-%m-%d %H:%M:%S).%ms [%V] %m%n"
[rules] [rules]
my_log.* "/home/orangepi/car/master/log/log_2026-06-25.log"; millisecond my_log.* "/home/orangepi/car/master/log/log_2026-06-29.log"; millisecond
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment