Skip to content

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

Merge branch 'feature/add_0207tankyouhua' into 'master'

Feature/add 0207tankyouhua See merge request !92
parents bd30e7cb cb35e55f
Show whitespace changes
Inline Side-by-side
Showing with 71 additions and 189 deletions
build/main
View file @ b77cdb0a
No preview for this file type
drivers/devicecontrol/car0101_control.c
View file @ b77cdb0a
...@@ -10,7 +10,7 @@ void car_calculate_L_R(int angle) { ...@@ -10,7 +10,7 @@ void car_calculate_L_R(int angle) {
} else if (angle > 180) { } else if (angle > 180) {
angle = 180; angle = 180;
} }
float pulse_width=(2 / 180.0) * angle + 0.5; float pulse_width=( angle / 180.0) *2 + 0.5;
// 周期（ms） // 周期（ms）
float period = 1000.0 / 50; float period = 1000.0 / 50;
int val = (int)((pulse_width / period) * 1000); int val = (int)((pulse_width / period) * 1000);
...@@ -23,139 +23,56 @@ void car0101_middle_pwm() {//车停止pwm值 ...@@ -23,139 +23,56 @@ void car0101_middle_pwm() {//车停止pwm值
car_calculate_L_R(90); car_calculate_L_R(90);
} }
/*车的速度转向函数*/ /*车的速度转向函数*/
void car0101_mode_1_flont(unsigned char gval) { void car0101_mode_1_flont(unsigned char gval) {
int a=0,b=1;
if (gval < 50) { if (gval < 50) {
pwmWrite(PWM_PIN_SPEED, 75); pwmWrite(PWM_PIN_SPEED, 75);
} else if (gval <= 55) {
pwmWrite(PWM_PIN_SPEED, 71);
} else if (gval <= 60) { } else if (gval <= 60) {
pwmWrite(PWM_PIN_SPEED, 70); pwmWrite(PWM_PIN_SPEED, 71-b+a);
} else if (gval <= 65) { }else if (gval <= 70) {
pwmWrite(PWM_PIN_SPEED, 70); pwmWrite(PWM_PIN_SPEED, 70-b+a);
} else if (gval <= 70) { } else if(gval >70){
pwmWrite(PWM_PIN_SPEED, 70); int back_speed = 70 - (gval-70)/10-b+a;
} else if (gval <= 75) { pwmWrite(PWM_PIN_SPEED, back_speed);
pwmWrite(PWM_PIN_SPEED, 69);
} else if (gval <= 90) {
pwmWrite(PWM_PIN_SPEED, 68);
} else if (gval <= 100) {
pwmWrite(PWM_PIN_SPEED, 68);
} else if (gval <= 110) {
pwmWrite(PWM_PIN_SPEED, 66);
} else if (gval <= 120) {
pwmWrite(PWM_PIN_SPEED, 65);
} else if (gval <= 130) {
pwmWrite(PWM_PIN_SPEED, 64);
} else if (gval <= 140) {
pwmWrite(PWM_PIN_SPEED, 62);
} else if (gval <= 150) {
pwmWrite(PWM_PIN_SPEED, 60);
} else if (gval <= 160) {
pwmWrite(PWM_PIN_SPEED, 59);
} else if (gval <= 170) {
pwmWrite(PWM_PIN_SPEED, 58);
} else if (gval <= 180) {
pwmWrite(PWM_PIN_SPEED, 57);
} else if (gval <= 190) {
pwmWrite(PWM_PIN_SPEED, 56);
} else if (gval <= 200) {
pwmWrite(PWM_PIN_SPEED, 55);
} }
} }
void car0101_mode_2_back(unsigned char gval) { void car0101_mode_2_back(unsigned char gval) {
if(gval<50) { int b=0,a=1;
pwmWrite(PWM_PIN_SPEED,75); if (gval < 50) {
}else if(50<=gval&&gval<=55) { pwmWrite(PWM_PIN_SPEED, 75);
pwmWrite(PWM_PIN_SPEED,76); }
}else if(55<gval&&gval<=60) { else if (gval <= 60) {
pwmWrite(PWM_PIN_SPEED,77); pwmWrite(PWM_PIN_SPEED, 79+b-a);
}else if(60<gval&&gval<=65) { }else if (gval <= 70) {
pwmWrite(PWM_PIN_SPEED,77); pwmWrite(PWM_PIN_SPEED, 80+b-a);
}else if(65<gval&&gval<=70) { }else if(gval >70){
pwmWrite(PWM_PIN_SPEED,78); int flont_speed = 80+(gval-70)/10+b-a;
}else if(70<gval&&gval<=75) { pwmWrite(PWM_PIN_SPEED, flont_speed);
pwmWrite(PWM_PIN_SPEED,78);
}else if(75<gval&&gval<=90) {
pwmWrite(PWM_PIN_SPEED,79);
}else if(90<gval&&gval<=100) {
pwmWrite(PWM_PIN_SPEED,80);
}else if(110<gval&&gval<=120) {
pwmWrite(PWM_PIN_SPEED,81);
}else if(120<gval&&gval<=130) {
pwmWrite(PWM_PIN_SPEED,82);
}else if(130<gval&&gval<=140) {
pwmWrite(PWM_PIN_SPEED,83);
}else if(140<gval&&gval<=150) {
pwmWrite(PWM_PIN_SPEED,84);
}else if(150<gval&&gval<=160) {
pwmWrite(PWM_PIN_SPEED,85);
}else if(160<gval&&gval<=170) {
pwmWrite(PWM_PIN_SPEED,86);
}else if(170<gval&&gval<=180) {
pwmWrite(PWM_PIN_SPEED,87);
}else if(180<gval&&gval<=190) {
pwmWrite(PWM_PIN_SPEED,88);
}else if(190<gval&&gval<=200) {
pwmWrite(PWM_PIN_SPEED,89);
} }
} }
void car0101_mode_3_left(unsigned char gval) { void car0101_mode_3_left(unsigned char gval) {
if (gval < 45) { int b=15;
if(gval<45){
car_calculate_L_R(90); car_calculate_L_R(90);
} else if (gval <= 51) { }else if(gval<70){
car_calculate_L_R(110); car_calculate_L_R(50+gval+b);
} else if (gval <= 57) { }else if(gval>=70){
car_calculate_L_R(120); car_calculate_L_R(35);
} else if (gval <= 63) {
car_calculate_L_R(130);
} else if (gval <= 69) {
car_calculate_L_R(130);
} else if (gval <= 75) {
car_calculate_L_R(140);
} else if (gval <= 81) {
car_calculate_L_R(145);
} else if (gval <= 87) {
car_calculate_L_R(150);
} else if (gval <= 93) {
car_calculate_L_R(150);
} else if (gval <= 100) {
car_calculate_L_R(160);
}else if (gval <= 107) {
car_calculate_L_R(170);
}else if (gval <= 120) {
car_calculate_L_R(180);
} }
} }
void car0101_mode_4_right(unsigned char gval) { void car0101_mode_4_right(unsigned char gval) {
if (gval < 45) { int b=10;
if(gval<45){
car_calculate_L_R(90); car_calculate_L_R(90);
} else if (gval <= 51) { }else if(gval<70){
car_calculate_L_R(70); car_calculate_L_R(130-gval-b);
} else if (gval <= 57) { }else if(gval>=70){
car_calculate_L_R(66); car_calculate_L_R(135);
} else if (gval <= 63) {
car_calculate_L_R(62);
} else if (gval <= 69) {
car_calculate_L_R(55);
} else if (gval <= 75) {
car_calculate_L_R(45);
} else if (gval <= 81) {
car_calculate_L_R(40);
} else if (gval <= 87) {
car_calculate_L_R(30);
} else if (gval <= 93) {
car_calculate_L_R(30);
} else if (gval <= 100) {
car_calculate_L_R(20);
} else if (gval <= 107) {
car_calculate_L_R(10);
} else if (gval <= 120) {
car_calculate_L_R(0);
} }
} }
......
drivers/devicecontrol/car0102_control.c
View file @ b77cdb0a
...@@ -118,68 +118,18 @@ void car0102_mode_3_left(unsigned char gval) { ...@@ -118,68 +118,18 @@ void car0102_mode_3_left(unsigned char gval) {
}else if(gval>=70){ }else if(gval>=70){
car0102_calculate_L_R(135); car0102_calculate_L_R(135);
} }
// if (gval < 45) {
// car0102_calculate_L_R(90);
// } else if (gval <= 51) {
// car0102_calculate_L_R(110);
// } else if (gval <= 57) {
// car0102_calculate_L_R(120);
// } else if (gval <= 63) {
// car0102_calculate_L_R(130);
// } else if (gval <= 69) {
// car0102_calculate_L_R(135);
// } else if (gval <= 75) {
// car0102_calculate_L_R(140);
// } else if (gval <= 81) {
// car0102_calculate_L_R(145);
// } else if (gval <= 87) {
// car0102_calculate_L_R(150);
// } else if (gval <= 93) {
// car0102_calculate_L_R(150);
// } else if (gval <= 100) {
// car0102_calculate_L_R(160);
// }else if (gval <= 107) {
// car0102_calculate_L_R(170);
// }else if (gval <= 120) {
// car0102_calculate_L_R(180);
// }
} }
void car0102_mode_4_right(unsigned char gval) { void car0102_mode_4_right(unsigned char gval) {
int b=5; int b=7;
if(gval<45){ if(gval<45){
car0102_calculate_L_R(90); car0102_calculate_L_R(90);
}else if(gval<70){ }else if(gval<70){
car0102_calculate_L_R(130-gval-b); car0102_calculate_L_R(130-gval-b);
}else if(gval>=70){ }else if(gval>=70){
car0102_calculate_L_R(135); car0102_calculate_L_R(45);
} }
// if (gval < 45) {
// car0102_calculate_L_R(90);
// } else if (gval <= 51) {
// car0102_calculate_L_R(70);
// } else if (gval <= 57) {
// car0102_calculate_L_R(66);
// } else if (gval <= 63) {
// car0102_calculate_L_R(62);
// } else if (gval <= 69) {
// car0102_calculate_L_R(55);
// } else if (gval <= 75) {
// car0102_calculate_L_R(45 );
// } else if (gval <= 81) {
// car0102_calculate_L_R(40);
// } else if (gval <= 87) {
// car0102_calculate_L_R(30);
// } else if (gval <= 93) {
// car0102_calculate_L_R(30);
// } else if (gval <= 100) {
// car0102_calculate_L_R(20);
// } else if (gval <= 107) {
// car0102_calculate_L_R(10);
// } else if (gval <= 120) {
// car0102_calculate_L_R(0);
// }
} }
//车速度和转向引脚数值处理函数 //车速度和转向引脚数值处理函数
......
drivers/devicecontrol/tank0207_control.c
View file @ b77cdb0a
...@@ -16,9 +16,9 @@ void tank0207_mode_lift_flont(unsigned char gval) { ...@@ -16,9 +16,9 @@ void tank0207_mode_lift_flont(unsigned char gval) {
else if (gval <= 60) { else if (gval <= 60) {
pwmWrite(PWM_PIN_SPEED, 80); pwmWrite(PWM_PIN_SPEED, 80);
}else if (gval <= 90) { }else if (gval <= 90) {
pwmWrite(PWM_PIN_SPEED, 79); pwmWrite(PWM_PIN_SPEED, 81);
}else if (gval <= 100) { }else if (gval <= 100) {
pwmWrite(PWM_PIN_SPEED, 79); pwmWrite(PWM_PIN_SPEED, 81);
}else if(gval >100){ }else if(gval >100){
int flont_speed = 80+(gval-70)/10+b; int flont_speed = 80+(gval-70)/10+b;
if(flont_speed>95) flont_speed = 95; if(flont_speed>95) flont_speed = 95;
...@@ -33,9 +33,9 @@ void tank0207_mode_lift_back(unsigned char gval) { ...@@ -33,9 +33,9 @@ void tank0207_mode_lift_back(unsigned char gval) {
} else if (gval <= 60) { } else if (gval <= 60) {
pwmWrite(PWM_PIN_SPEED, 70); pwmWrite(PWM_PIN_SPEED, 70);
}else if (gval <= 90) { }else if (gval <= 90) {
pwmWrite(PWM_PIN_SPEED, 71); pwmWrite(PWM_PIN_SPEED, 69);
}else if (gval <= 100) { }else if (gval <= 100) {
pwmWrite(PWM_PIN_SPEED, 71); pwmWrite(PWM_PIN_SPEED, 69);
} else if(gval >100){ } else if(gval >100){
int back_speed = 71 - (gval-70)/10-b; int back_speed = 71 - (gval-70)/10-b;
if(back_speed<55) back_speed =55; if(back_speed<55) back_speed =55;
...@@ -52,9 +52,9 @@ void tank0207_mode_right_flont(unsigned char gval) { ...@@ -52,9 +52,9 @@ void tank0207_mode_right_flont(unsigned char gval) {
else if (gval <= 60) { else if (gval <= 60) {
pwmWrite(PWM_PIN_CHANGE, 80); pwmWrite(PWM_PIN_CHANGE, 80);
}else if (gval <= 90) { }else if (gval <= 90) {
pwmWrite(PWM_PIN_CHANGE, 79); pwmWrite(PWM_PIN_CHANGE, 69);
}else if (gval <= 100) { }else if (gval <= 100) {
pwmWrite(PWM_PIN_CHANGE, 79); pwmWrite(PWM_PIN_CHANGE, 69);
}else if(gval >100){ }else if(gval >100){
int flont_speed = 80+(gval-70)/10+b; int flont_speed = 80+(gval-70)/10+b;
if(flont_speed>95) flont_speed = 95; if(flont_speed>95) flont_speed = 95;
...@@ -162,15 +162,15 @@ void tank0207_change(unsigned char *buf) { ...@@ -162,15 +162,15 @@ void tank0207_change(unsigned char *buf) {
} }
else if(tank0207_front_t ==1&&tank0207_steering_t==1){ else if(tank0207_front_t ==1&&tank0207_steering_t==1){
tank0207_mode_lift_flont(0); tank0207_mode_lift_flont(0);
tank0207_mode_right_flont(tank0207_steering_val+tank0206_count); tank0207_mode_right_flont(tank0207_front_val);
}else if(tank0207_front_t ==1&&tank0207_steering_t==2){ }else if(tank0207_front_t ==1&&tank0207_steering_t==2){
tank0207_mode_lift_flont(tank0207_steering_val+tank0206_count); tank0207_mode_lift_flont(tank0207_front_val);
tank0207_mode_right_flont(0); tank0207_mode_right_flont(0);
}else if(tank0207_front_t ==2&&tank0207_steering_t==1){ }else if(tank0207_front_t ==2&&tank0207_steering_t==1){
tank0207_mode_lift_back(0); tank0207_mode_lift_back(0);
tank0207_mode_right_back(tank0207_steering_val+tank0206_count); tank0207_mode_right_back(tank0207_front_val);
}else if(tank0207_front_t ==2&&tank0207_steering_t==2){ }else if(tank0207_front_t ==2&&tank0207_steering_t==2){
tank0207_mode_lift_back(tank0207_steering_val+tank0206_count); tank0207_mode_lift_back(tank0207_front_val);
tank0207_mode_right_back(0); tank0207_mode_right_back(0);
} }
......
drivers/devicecontrol/tank_common.c
View file @ b77cdb0a
...@@ -134,13 +134,15 @@ static void tankshot_detectionback(){ ...@@ -134,13 +134,15 @@ static void tankshot_detectionback(){
if(digitalRead(12)==LOW) { if(digitalRead(12)==LOW) {
s_back_status=true; s_back_status=true;
index=1; index=1;
g_device_delay_back_count=0;
delay_ms(100); // 延时 100ms，防止抖动 delay_ms(100); // 延时 100ms，防止抖动
g_device_delay_back_count=0;
softPwmWrite(27, 0); softPwmWrite(27, 0);
set_backshotstatus(false); set_backshotstatus(false);
my_zlog_info("检测引脚12拉低"); my_zlog_info("检测引脚12拉低");
} }
if(s_back_status==true&&index==1) tank_delay_shot_back(); if(s_back_status==true&&index==1) {
tank_delay_shot_back();
}
tankshot_detectionback_count++; tankshot_detectionback_count++;
if(tankshot_detectionback_count>3000){ if(tankshot_detectionback_count>3000){
softPwmWrite(27, 0); softPwmWrite(27, 0);
......
drivers/gpio/gpio_control.c
View file @ b77cdb0a
...@@ -176,23 +176,29 @@ void tank_angle_limit_function(){ ...@@ -176,23 +176,29 @@ void tank_angle_limit_function(){
limit_status = angle_limit(); limit_status = angle_limit();
if(limit_status==LIMIT_LIFT) { if(limit_status==LIMIT_LIFT) {
device_gpio_control(g_device_type,5,0); device_gpio_control(g_device_type,5,0);
my_zlog_info("lift limit stop"); if(limit_log_count>100){
my_zlog_info("right limit");
limit_log_count=0;
}
} }
else if(limit_status==LIMIT_RIGHT) { else if(limit_status==LIMIT_RIGHT) {
device_gpio_control(g_device_type,7,0); device_gpio_control(g_device_type,7,0);
my_zlog_info("right limit stop"); if(limit_log_count>100){
my_zlog_info("lift limit");
limit_log_count=0;
}
} }
if(limit_status==0) { else if(limit_status==0) {
limit_log_count++; limit_log_count++;
if(limit_log_count>100){ if(limit_log_count>100){
my_zlog_info("limit stop"); my_zlog_info("limit stop");
limit_log_count=0; limit_log_count=0;
} }
delay_ms(5);
} }
delay_ms(8);
} }
} }
...@@ -580,7 +586,7 @@ void tank0204_pwm_value(int pin,int value){ ...@@ -580,7 +586,7 @@ void tank0204_pwm_value(int pin,int value){
//device_shoting_check(27,100); //device_shoting_check(27,100);
if(getshot_detection_index()==true){ if(getshot_detection_index()==true){
softPwmWrite(27, 100); softPwmWrite(27, 100);
setshot_detection_index(true); setshot_detection_index(false);
tankshot_detection_backcount(NULL,3); tankshot_detection_backcount(NULL,3);
set_backshotstatus(true); set_backshotstatus(true);
my_zlog_info("冷却完成，射击开始"); my_zlog_info("冷却完成，射击开始");
...@@ -594,7 +600,7 @@ void tank0204_pwm_value(int pin,int value){ ...@@ -594,7 +600,7 @@ void tank0204_pwm_value(int pin,int value){
my_zlog_info("pwm:5,1"); my_zlog_info("pwm:5,1");
}else { }else {
if(pin==26&&get_backshotstatus()!=true) { if(pin==26&&get_backshotstatus()!=true) {
devicegpio_pwmreuse(pin,value,100); devicegpio_pwmreuse(pin,value,60);
softPwmWrite(pin, 60); softPwmWrite(pin, 60);
} }
if(pin!=26)softPwmWrite(pin, 60); if(pin!=26)softPwmWrite(pin, 60);
......
drivers/sensors/tank_angle.c
View file @ b77cdb0a
...@@ -15,7 +15,7 @@ double tank_angle(){ ...@@ -15,7 +15,7 @@ double tank_angle(){
int angle_limit(){ int angle_limit(){
if(ANGLE_LIMIT_INDEX == 1 ){ if(ANGLE_LIMIT_INDEX == 1 ){
int tank_limit_angele = tank_angle(); int tank_limit_angele = (int)tank_angle();
if(tank_limit_angele >=LIFT_LIMIT && tank_limit_angele<=MIDDLE_LIMIT) return 1; if(tank_limit_angele >=LIFT_LIMIT && tank_limit_angele<=MIDDLE_LIMIT) return 1;
else if(tank_limit_angele >=MIDDLE_LIMIT && tank_limit_angele<=RIGHT_LIMIT) return 2; else if(tank_limit_angele >=MIDDLE_LIMIT && tank_limit_angele<=RIGHT_LIMIT) return 2;
return 0; return 0;
......
drivers/sensors/tank_angle.h
View file @ b77cdb0a
...@@ -5,12 +5,14 @@ ...@@ -5,12 +5,14 @@
#define MIDDLE_LIMIT 180 #define MIDDLE_LIMIT 180
#define RIGHT_LIMIT 200 #define RIGHT_LIMIT 200
#define ANGLE_LIMIT_INDEX 1 //是否开启角度旋转 #define ANGLE_LIMIT_INDEX 1 //是否开启角度限制
double tank_angle(); double tank_angle();
int angle_limit(); int angle_limit();
void settank_angle(double angle);
float low_pass_filter_360(float prev_angle, float new_angle, float alpha); float low_pass_filter_360(float prev_angle, float new_angle, float alpha);
void set_tank_angle_count();//计时 void set_tank_angle_count();//计时
......
modules/mqtt/mqtt_infor_handle.c
View file @ b77cdb0a
...@@ -78,9 +78,14 @@ void angle_mqtt_send() { ...@@ -78,9 +78,14 @@ void angle_mqtt_send() {
char TOPIC_send_angle[26]; char TOPIC_send_angle[26];
static double prev_angle=0; static double prev_angle=0;
static double rounded_angle; static double rounded_angle;
rounded_angle =low_pass_filter_360(prev_angle,tank_angle(),0.8); rounded_angle =low_pass_filter_360(prev_angle,tank_angle(),0.7);
sprintf(TOPIC_send_angle,"dev_rtinfo/%s",mqtt_topic_pure_number()); sprintf(TOPIC_send_angle,"dev_rtinfo/%s",mqtt_topic_pure_number());
cJSON_AddStringToObject(root, "type","tank_angle"); cJSON_AddStringToObject(root, "type","tank_angle");
if(rounded_angle>160&&rounded_angle<180){
rounded_angle=160;
}else if(rounded_angle<200&&rounded_angle>180){
rounded_angle=200;
}
cJSON_AddNumberToObject(root, "angle",rounded_angle); cJSON_AddNumberToObject(root, "angle",rounded_angle);
char *payload = cJSON_PrintUnformatted(root); char *payload = cJSON_PrintUnformatted(root);
angle_i++; angle_i++;
......
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