Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
//在中断中执行 /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);
    }
}