TI MSPM0 PID控制角度旋转走8字

雾削木

已于 2025-07-03 09:41:18 修改

阅读量119

点赞数 2

CC 4.0 BY-SA版权

分类专栏： TI 文章标签： java linux 前端

于 2025-07-03 09:41:04 首次发布

本文链接：https://blog.csdn.net/WuXiaoMuDeBug/article/details/149087464

TI 专栏收录该内容

16 篇文章

订阅专栏

		
		if(flag == 1){
    switch (tag)
    {
    case 0:
            speedA = PID(35,0, target,Angle[0]);
            speedB = 300;
            motor_set(speedA,speedB);
//                if(target == -35.0f){
//                    while(Angle[0]>target){
//                        motor_set(200,0);
//                    }
//                    
//                }else{
//                    while(Angle[0]<target - 5.0f || Angle[0] >= 0){
//                        motor_set(0,200);
//                    }
//                    while(Angle[0]>target + 5.0f){
//                        motor_set(200,0);
//                    }
//                }
//                motor_set(200,200);
                Trace_get();
                if(Trace[0]>0||Trace[1]>0||Trace[2]>0||Trace[3]>0||Trace[4]>0||Trace[5]>0||Trace[6]>0||Trace[7]>0)
                {
                    motor_set(0,0);
                    if(target == 145.0f){
                        motor_set(150,0);
                    }else{
                        motor_set(0,150);
                    }
                    DL_GPIO_clearPins(GPIO_PORT,GPIO_BEEP_PIN);
                    delay_ms(500);
                    DL_GPIO_setPins(GPIO_PORT, GPIO_BEEP_PIN);
                    
                    tag = 1;
                }
        break;
    case 1:
                    
                position = xunji();
                PID_data = vertical_PID_Turn(position, 0);
                speedA = 250 + PID_data;
                speedB = 250 - PID_data;
                motor_set(speedA, speedB);
                if(position != 0){
                    last = position;
                }
                if(Trace[0]==0&&Trace[1]==0&&Trace[2]==0&&Trace[3]==0&&Trace[4]==0&&Trace[5]==0&&Trace[6]==0&&Trace[7]==0)
                {
                    delay_ms(200);
                    if(Trace[0]==0&&Trace[1]==0&&Trace[2]==0&&Trace[3]==0&&Trace[4]==0&&Trace[5]==0&&Trace[6]==0&&Trace[7]==0)
                    {
                    if(target == 40.0f){
                        motor_set(0,140);
                    }else{
                        motor_set(140,0);
                    }
                    DL_GPIO_clearPins(GPIO_PORT,GPIO_BEEP_PIN);
                    delay_ms(500);
                    motor_set(0,0);
                    DL_GPIO_setPins(GPIO_PORT, GPIO_BEEP_PIN);
                    if(target == 40.0f){
                        target = 145.0f;
                    }else if(target == 145.0f){
                        target = 40.0f;
                    }
                    
                    tag = 0;
                    }
                }

        break;
    default:
        break;
    }
    
}else if(flag == 2){
    switch (tag)
    {
    case 0:
        tag=0;
        // 获取循迹传感器的位置偏差
        position = xunji();
        PID_data = vertical_PID_Turn(position, 0);
        speedA = 250 + PID_data;
        speedB = 250 - PID_data;
        motor_set(speedA, speedB);
        if(Trace[0]==0&&Trace[1]==0&&Trace[2]==0&&Trace[3]==0&&Trace[4]==0&&Trace[5]==0&&Trace[6]==0&&Trace[7]==0){
            tag=1;
        }
        break;
    case 1:
        motor_set(0,0);
        DL_GPIO_clearPins(GPIO_PORT,GPIO_BEEP_PIN);
        delay_ms(500);
        DL_GPIO_setPins(GPIO_PORT, GPIO_BEEP_PIN);
        tag = 2;
        break;
    default:
        break;
    }
    
}else{

pid.c

int16_t PID(float kp,float ki,double traget , double previous)
{
	
	uint16_t pwm_val1;
	int16_t e;
	static int16_t E;

	
	e=traget+previous;
	E=E+e;
	if(E>100)E=100;
	if(E<-100)E=-100;
	pwm_val1=kp*(e)+ki*(E+e)+300;      //200是左轮电机的初始PWM值
	if(pwm_val1>400)pwm_val1=400;      //对电机的PWM值限幅，250是周期上线，150是电机旋转的最小PWM值（最小占空比）
	if(pwm_val1<100)pwm_val1=100;
	  
	return pwm_val1;
}
int16_t PID1(float kp,float ki,double traget , double previous)
{
	
	uint16_t pwm_val1;
	int16_t e;
	static int16_t E;

	
	e=traget+previous;
	E=E+e;
	if(E>100)E=100;
	if(E<-100)E=-100;
	pwm_val1=kp*(e)+ki*(E+e)+400;      //200是左轮电机的初始PWM值
	if(pwm_val1>550)pwm_val1=550;      //对电机的PWM值限幅，250是周期上线，150是电机旋转的最小PWM值（最小占空比）
	if(pwm_val1<100)pwm_val1=100;
	  
	return pwm_val1;
}