diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index e3db4ba..ee8ea09 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -56,15 +56,15 @@ static referee_info_t* referee_data; // 用于获取裁判系统的数据 static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI static SuperCapInstance *cap; // 超级电容 -static LKMotorInstance *motor_lf,*motor_rb; // 轮电机9015 -static DJIMotorInstance *motor_steering_lf,*motor_steering_rb; // 舵电机6020 +static LKMotorInstance *motor_lb,*motor_rf; // 轮电机9015 +static DJIMotorInstance *motor_steering_lb,*motor_steering_rf; // 舵电机6020 /* 用于自旋变速策略的时间变量 */ // static float t; /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */ static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘 -static float vt_lf, vt_rb; // 底盘速度解算后的临时输出,待进行限幅 -static float at_lf, at_rb; // 底盘的角度解算后的临时输出,待进行限幅 +static float vt_lb, vt_rf; // 底盘速度解算后的临时输出,待进行限幅 +static float at_lb, at_rf; // 底盘的角度解算后的临时输出,待进行限幅 static PIDInstance chassis_follow_pid; // 底盘跟随云台的PID void ChassisInit() @@ -100,13 +100,13 @@ void ChassisInit() }; // @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改. - chassis_motor_config.can_init_config.tx_id = 1; - // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_rb = LKMotorInit(&chassis_motor_config); - chassis_motor_config.can_init_config.tx_id = 2; // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - motor_lf = LKMotorInit(&chassis_motor_config); + motor_rf = LKMotorInit(&chassis_motor_config); + + chassis_motor_config.can_init_config.tx_id = 1; + // chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; + motor_lb = LKMotorInit(&chassis_motor_config); // 舵电机6020初始化 Motor_Init_Config_s chassis_motor_steering_config = { @@ -145,10 +145,10 @@ void ChassisInit() }; chassis_motor_steering_config.can_init_config.tx_id = 1; - motor_steering_rb = DJIMotorInit(&chassis_motor_steering_config); + motor_steering_rf = DJIMotorInit(&chassis_motor_steering_config); chassis_motor_steering_config.can_init_config.tx_id = 2; - motor_steering_lf = DJIMotorInit(&chassis_motor_steering_config); + motor_steering_lb = DJIMotorInit(&chassis_motor_steering_config); PID_Init_Config_s chassis_follow_pid_conf = { .Kp = 6, // 6 @@ -266,46 +266,58 @@ static void MinmizeRotation(float *angle, const float *last_angle, float *speed) * @brief 舵轮电机角度解算 * */ -static void SteeringWheelCalculate() + static void SteeringWheelCalculate() { - float offset_lf, offset_rb; // 用于计算舵轮的角度 - float at_lf_last, at_rb_last; // 上次的角度 - at_lf_last = motor_steering_lf->measure.total_angle; - at_rb_last = motor_steering_rb->measure.total_angle; + float offset_rf, offset_lb; // 用于计算舵轮的角度 + float at_rf_last, at_lb_last; // 上次的角度 + at_lb_last = motor_steering_lb->measure.total_angle; + // at_lf_last = motor_steering_lf->measure.total_angle; + at_rf_last = motor_steering_rf->measure.total_angle; + // at_rb_last = motor_steering_rb->measure.total_angle; if (chassis_vx == 0 && chassis_vy == 0 && chassis_cmd_recv.wz == 0) { // at_lf = at_lf_last; // at_rf = at_rf_last; // at_lb = at_lb_last; // at_rb = at_rb_last; - vt_lf = 0, vt_rb = 0; + vt_lb = 0, vt_rf = 0; } else { // 生成预计算变量，减少计算量，空间换时间 float w = chassis_cmd_recv.wz * CHASSIS_WHEEL_OFFSET * SQRT2; float temp_x = chassis_vx - w, temp_y = chassis_vy - w; - arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x- - temp_y = chassis_vy + w; // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释 - // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x- - // temp_x = chassis_vx + w; // temp_y = chassis_vx + w;与上次相同因此注释 - arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+ - temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释 - // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+ + // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lf); // lf：y- , x- + // temp_y = chassis_vy + w; // 重复利用变量,temp_x = chassis_vy - w;与上次相同因此注释 + arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_lb); // lb: y+ , x- + temp_x = chassis_vx + w; // temp_y = chassis_vx + w;与上次相同因此注释 + // arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rb); // rb: y+ , x+ + // temp_y = chassis_vy - w; // temp_x = chassis_vy + w;与上次相同因此注释 + arm_sqrt_f32(temp_x * temp_x + temp_y * temp_y, &vt_rf); // rf: y- , x+ // 计算角度偏移 - offset_lf = atan2f(chassis_vx - w, chassis_vy - w) * RAD_2_DEGREE; // lf: y- , x- - offset_rb = atan2f(chassis_vx + w, chassis_vy + w) * RAD_2_DEGREE; // rb: y+ , x+ + // offset_lf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // lf: y- , x- + offset_rf = atan2f(chassis_vy - w, chassis_vx - w) * RAD_2_DEGREE; // rf: y- , x+ + offset_lb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // lb: y+ , x- + // offset_rb = atan2f(chassis_vy + w, chassis_vx + w) * RAD_2_DEGREE; // rb: y+ , x+ - at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度 - at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb; + // at_lf = STEERING_CHASSIS_ALIGN_ANGLE_LF + offset_lf; // 此处似乎不需要加上真实角度，因为我们需要的是绝对角度而不是相对角度 + at_rf = STEERING_CHASSIS_ALIGN_ANGLE_RF + offset_rf; + at_lb = STEERING_CHASSIS_ALIGN_ANGLE_LB + offset_lb; + // at_rb = STEERING_CHASSIS_ALIGN_ANGLE_RB + offset_rb; - ANGLE_LIMIT_360_TO_180_ABS(at_lf); - ANGLE_LIMIT_360_TO_180_ABS(at_rb); + // ANGLE_LIMIT_360_TO_180_ABS(at_lf); + ANGLE_LIMIT_360_TO_180_ABS(at_rf); + ANGLE_LIMIT_360_TO_180_ABS(at_lb); + // ANGLE_LIMIT_360_TO_180_ABS(at_rb); - MinmizeRotation(&at_lf, &at_lf_last, &vt_lf); - MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); + // MinmizeRotation(&at_lf, &at_lf_last, &vt_lf); + MinmizeRotation(&at_rf, &at_rf_last, &vt_rf); + MinmizeRotation(&at_lb, &at_lb_last, &vt_lb); + // MinmizeRotation(&at_rb, &at_rb_last, &vt_rb); } - DJIMotorSetRef(motor_steering_lf, at_lf); - DJIMotorSetRef(motor_steering_rb, at_rb); + // DJIMotorSetRef(motor_steering_lf, at_lf); + DJIMotorSetRef(motor_steering_rf, at_rf); + DJIMotorSetRef(motor_steering_lb, at_lb); + // DJIMotorSetRef(motor_steering_rb, at_rb); } /** @@ -335,8 +347,8 @@ static void LimitChassisOutput() // LKMotorSetRef( motor_lf , vt_lf ); // LKMotorSetRef( motor_rb , vt_rb ); - LKMotorSetRef( motor_lf , vt_lf ); - LKMotorSetRef( motor_rb , vt_rb ); + LKMotorSetRef( motor_rf , vt_rf ); + LKMotorSetRef( motor_lb , vt_lb ); } /** @@ -365,18 +377,18 @@ void ChassisTask() if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE) { // 如果出现重要模块离线或遥控器设置为急停,让电机停止 - LKMotorStop(motor_lf); - LKMotorStop(motor_rb); - DJIMotorStop(motor_steering_lf); - DJIMotorStop(motor_steering_rb); + LKMotorStop(motor_rf); + LKMotorStop(motor_lb); + DJIMotorStop(motor_steering_rf); + DJIMotorStop(motor_steering_lb); } else { // 正常工作 - LKMotorEnable(motor_lf); - LKMotorEnable(motor_rb); - DJIMotorEnable(motor_steering_lf); - DJIMotorEnable(motor_steering_rb); + LKMotorEnable(motor_rf); + LKMotorEnable(motor_lb); + DJIMotorEnable(motor_steering_rf); + DJIMotorEnable(motor_steering_lb); } // 根据控制模式设定旋转速度 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index bcf8eab..b2ea2d2 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -155,7 +155,7 @@ static void RemoteControlSet() } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘和云台分离,底盘保持不转动 { - chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; + chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE; } @@ -174,8 +174,8 @@ static void RemoteControlSet() // 云台软件限位 // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整 - chassis_cmd_send.vy = -0.15f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 - chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 + chassis_cmd_send.vx = 0.15f * (float)rc_data[TEMP].rc.rocker_r_; // _水平方向 + chassis_cmd_send.vy = 0.15f * (float)rc_data[TEMP].rc.rocker_r1; // 1数值方向 // // 发射参数 // if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 3a086e0..d4ecc3c 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -56,7 +56,7 @@ void GimbalInit() .speed_feedback_source = OTHER_FEED, .outer_loop_type = ANGLE_LOOP, .close_loop_type = ANGLE_LOOP | SPEED_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, + .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, }, .motor_type = GM6020}; // PITCH diff --git a/application/robot_def.h b/application/robot_def.h index 07ae8b0..a2f5f10 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -26,8 +26,8 @@ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ // 云台参数 -#define YAW_CHASSIS_ALIGN_ECD 8192 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 -#define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 +#define YAW_CHASSIS_ALIGN_ECD 3938 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 +#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define PITCH_HORIZON_ECD 3412 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 0 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE 0 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) @@ -35,11 +35,11 @@ // #define CHASSIS_OMNI_WHEEL // 是否为全向轮底盘 // #define CHASSIS_MCNAMEE_WHEEL // 是否为麦克纳姆轮底盘 #define CHASSIS_STEERING_WHEEL // 是否为舵轮 -#define STEERING_CHASSIS_ALIGN_ECD_LF 1221 // 舵电机 A 编码器值，若有机械改动需要修改 -#define STEERING_CHASSIS_ALIGN_ECD_RB 7294 // 舵电机 D 编码器值，若有机械改动需要修改 +#define STEERING_CHASSIS_ALIGN_ECD_RF 1221 // 舵电机 A 编码器值，若有机械改动需要修改 +#define STEERING_CHASSIS_ALIGN_ECD_LB 7294 // 舵电机 D 编码器值，若有机械改动需要修改 -#define STEERING_CHASSIS_ALIGN_ANGLE_LF STEERING_CHASSIS_ALIGN_ECD_LF / 8192.f * 360.f // 舵轮 A 对齐角度 -#define STEERING_CHASSIS_ALIGN_ANGLE_RB STEERING_CHASSIS_ALIGN_ECD_RB / 8192.f * 360.f // 舵轮 D 对齐角度 +#define STEERING_CHASSIS_ALIGN_ANGLE_RF STEERING_CHASSIS_ALIGN_ECD_RF / 8192.f * 360.f // 舵轮 A 对齐角度 +#define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度 // 发射参数 #define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f