btl143/QD4C-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

日常更新

Browse Source
This commit is contained in:
井明江
2024-06-16 20:49:57 +08:00
parent f33ab5ab4b
commit 8d4580f225
4 changed files with 60 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
app/jj_motion.c
View File

@@ -76,10 +76,10 @@ uint8_t in_state = 0;
uint8_t in_stop = 0;
uint8_t last_state = 0;
uint8_t stop_flag = 0;
uint32_t pwm_duty_ls;
uint32_t pwm_duty_rs;
uint32_t pwm_duty_lb;
uint32_t pwm_duty_rb;
int32_t pwm_duty_ls;
int32_t pwm_duty_rs;
int32_t pwm_duty_lb;
int32_t pwm_duty_rb;
static float myclip_f(float x, float low, float up)
{
@@ -108,10 +108,12 @@ void sport_motion()
imu660ra_get_gyro();
imu660ra_get_acc();
in_gyro = imu660ra_gyro_z;
// 停车
// 抖动停车
if (imu660ra_acc_z <= 1000) {
bt_fly_flag = bt_run_flag = 0;
}
// 斑马线停车
if (1 == in_stop) {
stop_flag = 1;
}
@@ -128,7 +130,7 @@ void sport_motion()
cnt5++;
// pid参数切换
if ((last_state != in_state) && (in_state == 0 || in_state == 2)) { // 直道
cnt3_flag = 0;
cnt3_flag = 1;
cnt3 = 0;
bt_printf("to 直道");
set_speed = set_speed1;
@@ -156,53 +158,37 @@ void sport_motion()
PID_SetTunings(&angle_pid, cn_Kp1, cn_Ki1, cn_Kd1);
}
last_state = in_state;
if (cnt3_flag == 1 && (in_state == 1 || in_state == 3)) {
cnt3++;
if (in_state == 3) {
if (cnt3 >= 1500) // 200ms
{
bt_printf("to 环内");
cnt3_flag = 2;
cnt3 = 0;
}
} else {
if (cnt3 >= 500) // 200ms
{
bt_printf("to 环内");
cnt3_flag = 2;
cnt3 = 0;
}
}
}
// pid计算
if (cnt1 >= 2) {
PID_Compute(&far_gyro_pid);
cnt1 = 0;
}
if (cnt2 >= 10) {
in_speed = sport_get_speed();
cnt2 = 0;
}
if (cnt5 >= 20) {
PID_Compute(&speed_pid);
PID_Compute(&far_angle_pid);
PID_Compute(&angle_pid);
cnt2 = 0;
}
if (cnt5 >= 20) {
cnt5 = 0;
}
// 输出限幅
if (in_state == 0 || in_state == 2) {
pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 7000.f);
pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 7000.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 5000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 5000.f);
pwm_duty_ls = (int32_t)myclip_f(-out_pos, 0.0f, 5000.f);
pwm_duty_rs = (int32_t)myclip_f(out_pos, 0.0f, 5000.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 7000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 7000.f);
} else if (in_state == 1 || in_state == 3) {
pwm_duty_ls = (int32_t)myclip_f(-out_pos, -7000.0f, 7000.f);
pwm_duty_rs = (int32_t)myclip_f(out_pos, -7000.0f, 7000.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 0.0f, 5000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 0.0f, 5000.f);
pwm_duty_ls = (int32_t)myclip_f(-out_pos, -8000.0f, 8000.f);
pwm_duty_rs = (int32_t)myclip_f(out_pos, -8000.0f, 8000.f);
pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, -7000.0f, 7000.f);
pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, -7000.0f, 7000.f);
}
by_pwm_power_duty((int32_t)pwm_duty_ls, (int32_t)pwm_duty_rs, (int32_t)pwm_duty_lb, (int32_t)pwm_duty_rb);
by_pwm_power_duty(pwm_duty_ls, pwm_duty_rs, pwm_duty_lb, pwm_duty_rb);
} else {
by_pwm_power_duty(0, 0, 0, 0);
}
@@ -211,9 +197,24 @@ void sport_motion()
by_pwm_update_duty(0 + 500, 0 + 500);
} else {
if ((in_state == 1 || in_state == 3) && cnt3_flag == 1) {
float tt =0;//60 * fabs(tanf(myclip_f(in_angle, -60.f, 60.f) / 180 * PI));
float tt = 100 * fabs(tanf(myclip_f(in_angle, -60.f, 60.f) / 180 * PI));
by_pwm_update_duty(bt_fly + 500 - tt, bt_fly + 500 - tt);
cnt3++;
if (in_state == 3) {
if (cnt3 >= 1200) // 200ms
{
bt_printf("to 环内");
cnt3_flag = 2;
cnt3 = 0;
}
} else {
if (cnt3 >= 400) // 200ms
{
bt_printf("to 弯道");
cnt3_flag = 2;
cnt3 = 0;
}
}
} else {
by_pwm_update_duty(bt_fly + 500, bt_fly + 500);
}
@@ -229,23 +230,23 @@ void sport_pid_init()
/* 角度控制 */
PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp1, an_Ki1, an_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&far_angle_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_angle_pid, 20);
PID_SetSampleTime(&far_angle_pid, 10);
PID_SetOutputLimits(&far_angle_pid, -4000.0f, 4000.0f);
PID(&angle_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
PID_SetMode(&angle_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&angle_pid, 20);
PID_SetOutputLimits(&angle_pid, -7000.0f, 7000.0f);
PID_SetSampleTime(&angle_pid, 10);
PID_SetOutputLimits(&angle_pid, -8000.0f, 8000.0f);
/* 角速度控制 */
PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); // m是增量
PID_SetMode(&far_gyro_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&far_gyro_pid, 20);
PID_SetOutputLimits(&far_gyro_pid, -5000.0f, 5000.0f);
PID_SetSampleTime(&far_gyro_pid, 10);
PID_SetOutputLimits(&far_gyro_pid, -7000.0f, 7000.0f);
/* 速度控制 */
PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, _PID_P_ON_M, _PID_CD_DIRECT);
PID_SetMode(&speed_pid, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&speed_pid, 20);
PID_SetOutputLimits(&speed_pid, -2000.0f, 4000.0f);
PID_SetSampleTime(&speed_pid, 10);
PID_SetOutputLimits(&speed_pid, -4000.0f, 6000.0f);
}

8
app/jj_motion.h
View File

@@ -45,10 +45,10 @@ extern float set_speed0;
extern float set_speed1;
extern float set_speed2;
extern float set_speed ;
extern uint32_t pwm_duty_ls;
extern uint32_t pwm_duty_rs;
extern uint32_t pwm_duty_lb;
extern uint32_t pwm_duty_rb;
extern int32_t pwm_duty_ls;
extern int32_t pwm_duty_rs;
extern int32_t pwm_duty_lb;
extern int32_t pwm_duty_rb;
extern uint8_t in_state;
extern uint8_t in_stop;

1
app/jj_voltage.c Normal file
View File

@@ -0,0 +1 @@
#include "jj_voltage.h"

7
app/jj_voltage.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef _JJ_VOLTAGE_H_
#define _JJ_VOLTAGE_H_
#include "stdio.h"
#include "zf_driver_uart.h"
#endif