void SetSpeed(void)
{
Motor_Control_0((s16)robotw.speed0);
Motor_Control_1((s16)robotw.speed1);
Motor_Control_2((s16)robotw.speed2);
Motor_Control_3((s16)robotw.speed3);
}
//在中断中执行 /5ms
void Car_Control(void){
imu_sensor_read_data_from_fifo(&sensor_saw_data,&sensor_data,&sensor_euler_angle);
if(CarRunning == 1) {
motor_output_Angle = Angle_Control_PD(sensor_euler_angle.roll,Car_Angle_Center, -(sensor_data.gyro[1] - 3.3f));
motor_output_Turn = Turn_Control(trun_mode,turn_target_speed,sensor_euler_angle.yaw,turn_target_orientaion);
motor_output_Speed = Speed_Control(speed_target);
int16_t motor1_output=motor_output_Angle-motor_output_Speed+motor_output_Turn;
int16_t motor2_output=motor_output_Angle-motor_output_Speed-motor_output_Turn;
Motor_Control_1(motor1_output);
Motor_Control_2(motor2_output);
}
else {
Motor_Control_1(0);
Motor_Control_2(0);
}
}