Skip to content

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

Merge branch 'feature/add_aboard_tts' into 'master'

修改了0107的运动相关代码 See merge request !127
parents 113bb6ad d49ec850
Show whitespace changes
Inline Side-by-side
Showing with 163 additions and 19 deletions
build/main
View file @ c4c1db77
No preview for this file type
drivers/devicecontrol/car/car0101_control.c
View file @ c4c1db77
......@@ -34,6 +34,9 @@ void car0101_mode_1_flont(int gval) {
pwmWrite(PWM_PIN_SPEED, 70-b+a);
} else if(gval >70){
int back_speed = 70 - (gval-70)/10-b+a;
if (back_speed < 55) {
back_speed = 55;
}
pwmWrite(PWM_PIN_SPEED, back_speed);
}
}
......@@ -49,6 +52,9 @@ void car0101_mode_2_back(int gval) {
pwmWrite(PWM_PIN_SPEED, 80+b-a);
}else if(gval >70){
int flont_speed = 80+(gval-70)/10+b-a;
if (flont_speed > 95) {
flont_speed = 95;
}
pwmWrite(PWM_PIN_SPEED, flont_speed);
}
}
......
drivers/devicecontrol/car/car0103_control.c
View file @ c4c1db77
......@@ -19,6 +19,9 @@ void mode_car0103_lift_flont(int gval) {
pwmWrite(PWM_PIN_SPEED, 80+b);
}else if(gval >70){
int flont_speed = 80+(gval-70)/10+b;
if (flont_speed > 95) {
flont_speed = 95;
}
pwmWrite(PWM_PIN_SPEED, flont_speed);
}
}
......@@ -33,6 +36,9 @@ void mode_car0103_lift_back(int gval) {
pwmWrite(PWM_PIN_SPEED, 70-b);
} else if(gval >70){
int back_speed = 70 - (gval-70)/10-b;
if (back_speed < 55) {
back_speed = 55;
}
pwmWrite(PWM_PIN_SPEED, back_speed);
}
}
......@@ -47,6 +53,9 @@ void mode_car0103_right_flont(int gval) {
pwmWrite(PWM_PIN_CHANGE, 80+b);
}else if(gval >70){
int flont_speed = 80+(gval-70)/10+b;
if (flont_speed > 95) {
flont_speed = 95;
}
pwmWrite(PWM_PIN_CHANGE, flont_speed);
}
}
......@@ -61,6 +70,9 @@ void mode_car0103_right_back(int gval) {
pwmWrite(PWM_PIN_CHANGE, 70-b);
} else if(gval >70){
int back_speed = 70 - (gval-70)/10-b;
if (back_speed < 55) {
back_speed = 55;
}
pwmWrite(PWM_PIN_CHANGE, back_speed);
}
}
......
drivers/devicecontrol/car/car0107_control.c
View file @ c4c1db77
......@@ -21,13 +21,54 @@ static int s_current_speed = 0;
static int s_speed_mode = 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_RETURN 90
#define REV_TARGET_SCALE_NUM 6
#define REV_TARGET_SCALE_DEN 10
#define ACCEL_STEP 5
#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) {
if (angle < 0) {
......@@ -54,26 +95,34 @@ static void car0107_reset_steering_state(void) {
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) {
if (speed <= 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) {
gval = STEER_GVAL_MAX;
}
return gval;
}
static void car0107_apply_stop(void) {
pwmWrite(PWM_PIN_SPEED, 75);
car0107_calculate_L_R(90);
}
/*车中间值，停止舵机中间函数*/
void car0107_middle_pwm() {
pthread_mutex_lock(&s_car0107_hw_mutex);
car0107_reset_steering_state();
car0107_reset_phone_state();
car0107_apply_stop();
pthread_mutex_unlock(&s_car0107_hw_mutex);
}
......@@ -88,6 +137,9 @@ static void car0107_mode_1_flont(int gval) {
pwmWrite(PWM_PIN_SPEED, 70-b+a);
} else if(gval >70){
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);
}
}
......@@ -103,6 +155,9 @@ static void car0107_mode_2_back(int gval) {
pwmWrite(PWM_PIN_SPEED, 80+b-a);
}else if(gval >70){
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);
}
}
......@@ -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) {
int mode=buf[1];
int val=buf[2];
int val=car0107_input_to_gval(buf[2]);
pthread_mutex_lock(&s_car0107_hw_mutex);
s_control_source = CTRL_PHONE;
s_target_speed = 0;
s_current_speed = 0;
s_speed_mode = 0;
s_speed_timeout = 0;
s_steering_timeout = 0;
switch(mode){
case 1:
car0107_mode_2_back(val);
car0107_set_phone_speed_command(1, val);
break;
case 2:
car0107_mode_1_flont(val);
car0107_set_phone_speed_command(2, val);
break;
case 3:
car0107_mode_3_left(val);
......@@ -158,6 +254,36 @@ void car0107_control_change(int *buf) {
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) {
if (s_control_source != CTRL_STEERING) {
return;
......@@ -257,6 +383,7 @@ static void car0107_steering_angle_smooth_process(void) {
void car0107_steering_smooth_all(void) {
pthread_mutex_lock(&s_car0107_hw_mutex);
car0107_phone_smooth_process();
car0107_speed_smooth_process();
car0107_steering_angle_smooth_process();
pthread_mutex_unlock(&s_car0107_hw_mutex);
......@@ -268,32 +395,31 @@ void car0107_control_steering_change_process(int *buf) {
pthread_mutex_lock(&s_car0107_hw_mutex);
s_control_source = CTRL_STEERING;
car0107_reset_phone_state();
s_steering_timeout = 200;
s_speed_timeout = 200;
if (mode == 1) {
s_speed_mode = 1;
if (val < 50) {
val = car0107_clamp_input(val);
if (val <= CAR0107_ACTIVE_THRESHOLD) {
s_target_speed = 0;
s_current_speed = 0;
s_speed_mode = 0;
car0107_apply_stop();
} else if (val <= 200) {
s_target_speed = val - 40;
} else {
s_target_speed = 160;
s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_BACK);
}
} else if (mode == 2) {
s_speed_mode = 2;
if (val < 50) {
val = car0107_clamp_input(val);
if (val <= CAR0107_ACTIVE_THRESHOLD) {
s_target_speed = 0;
s_current_speed = 0;
s_speed_mode = 0;
car0107_apply_stop();
} else if (val <= 200) {
s_target_speed = (val - 40) * 3 / 4;
} else {
s_target_speed = 80;
s_target_speed = car0107_input_to_target_speed(val, CAR0107_MAX_TARGET_FRONT);
}
} else if (mode == 3) {
if (val == 0) {
......
zlog.conf
View file @ c4c1db77
......@@ -9,4 +9,4 @@ file perms = 600
millisecond = "%d(%Y-%m-%d %H:%M:%S).%ms [%V] %m%n"
[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