diff --git a/app/by_frame.c b/app/by_frame.c index 70baa6f..1fb8160 100644 --- a/app/by_frame.c +++ b/app/by_frame.c @@ -49,7 +49,7 @@ void by_frame_parse(uint8_t data_num, uint32_t *data_array) uint16_t crc_cal = 0; uint32_t read_length = 50; - if (fifo_used(&frame_fifo)) { + if (fifo_used(&frame_fifo) >= 4 + 4 * data_num) { fifo_read_buffer(&frame_fifo, frame_buffer_parse, &read_length, FIFO_READ_AND_CLEAN); while (1) { if (cnt_ptr < 50) { diff --git a/app/jj_motion.c b/app/jj_motion.c index f752022..28d0a69 100644 --- a/app/jj_motion.c +++ b/app/jj_motion.c @@ -39,7 +39,7 @@ float out_speed; float set_speed = 0.0f; int cnt1 = 0; - +int cnt2 = 0; float sport_get_speed(void) { #define ALPHA (0.97f) @@ -60,6 +60,7 @@ void sport_motion(void) { cnt1++; + cnt2++; imu660ra_get_gyro(); in_gyro = imu660ra_gyro_z; // 陀螺仪输入 // in_angle = 0; // 图像远端输入 @@ -68,20 +69,33 @@ void sport_motion(void) PID_Compute(&far_gyro_pid); if (cnt1 >= 10) { - PID_Compute(&far_angle_pid); + PID_Compute(&speed_pid); in_speed = -sport_get_speed(); // printf("rate:%d\r\n", (int16_t)in_speed); PID_Compute(&near_pos_pid); + PID_Compute(&near_pos_pid); cnt1 = 0; } - + if (cnt2 >= 20) { + cnt2 = 0; + PID_Compute(&far_angle_pid); + } if (bt_run_flag == 1) { - // by_pwm_power_duty((int32_t)(500 + out_pos + out_gyro), - // (int32_t)(500 - out_pos - out_gyro), - // (int32_t)(500 + out_speed + out_gyro), - // (int32_t)(500 + out_speed - out_gyro)); - by_pwm_power_duty(500, 500, 500 + out_speed, 500 + out_speed); + if (out_gyro > 0) { + by_pwm_power_duty((int32_t)(500 - 1 * out_pos + 0 * out_gyro), + (int32_t)(500 + 1 * out_pos - 0 * out_gyro), + (int32_t)(500 + out_speed + out_gyro), + (int32_t)(500 + out_speed - out_gyro)); + + } else if (out_gyro <= 0) { + by_pwm_power_duty((int32_t)(500 - 1 * out_pos + 0 * out_gyro), + (int32_t)(500 + 1 * out_pos - 0 * out_gyro), + (int32_t)(500 + out_speed + out_gyro), + (int32_t)(500 + out_speed - out_gyro)); + } + + // by_pwm_power_duty(500+out_gyro, 500-out_gyro, 500+out_gyro , 500-out_gyro); } else { by_pwm_power_duty(500, 500, 500, 500); } @@ -99,7 +113,7 @@ void sport_motion(void) void sport_pid_init() { - PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp, an_Ki, an_Kd, 0, 1); + PID(&far_angle_pid, &in_angle, &out_angle, &set_angle, an_Kp, an_Ki, an_Kd, 1, 1); PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp, gy_Ki, gy_Kd, 1, 1); PID_Init(&far_angle_pid); PID_Init(&far_gyro_pid); @@ -110,13 +124,13 @@ void sport_pid_init() PID_SetMode(&far_gyro_pid, 1); PID_SetMode(&far_angle_pid, 1); - PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp, po_Ki, po_Kd, 0, 0); + PID(&near_pos_pid, &in_pos, &out_pos, &set_pos, po_Kp, po_Ki, po_Kd, 1, 0); PID(&speed_pid, &in_speed, &out_speed, &set_speed, sp_Kp, sp_Ki, sp_Kd, 1, 0); PID_Init(&near_pos_pid); PID_Init(&speed_pid); PID_SetOutputLimits(&near_pos_pid, -500.0f, 500.0f); - PID_SetOutputLimits(&speed_pid, -400.0f, 400.0f); + PID_SetOutputLimits(&speed_pid, 0.0f, 400.0f); PID_SetMode(&near_pos_pid, 1); PID_SetMode(&speed_pid, 1); diff --git a/app/jj_param.c b/app/jj_param.c index 4d96041..6f4e0f3 100644 --- a/app/jj_param.c +++ b/app/jj_param.c @@ -21,9 +21,9 @@ void jj_param_eeprom_init(void) PARAM_REG(angle_Ki, &an_Ki, EFLOAT, 1, "an_I:"); // 注冊 PARAM_REG(angle_Kd, &an_Kd, EFLOAT, 1, "an_D:"); - PARAM_REG(gyro_Kp, &gy_Kp, EFLOAT, 1, "im_P:"); // 注冊 - PARAM_REG(gyro_Ki, &gy_Ki, EFLOAT, 1, "im_I:"); // 注冊 - PARAM_REG(gyro_Kd, &gy_Kd, EFLOAT, 1, "im_D:"); + PARAM_REG(gyro_Kp, &gy_Kp, EFLOAT, 1, "gy_P:"); // 注冊 + PARAM_REG(gyro_Ki, &gy_Ki, EFLOAT, 1, "gy_I:"); // 注冊 + PARAM_REG(gyro_Kd, &gy_Kd, EFLOAT, 1, "gy_D:"); PARAM_REG(speed_Kp, &sp_Kp, EFLOAT, 1, "sp_P:"); // 注冊 PARAM_REG(speed_Ki, &sp_Ki, EFLOAT, 1, "sp_I:"); // 注冊