void SpeedY_Control() { if( yStatus == 0) { Position_PID(&DistanceY,xCarParam.EncoderSumY,xCarParam.CarDistanceY); xCarParam.Speed_Y = DistanceY.result; if(xCarParam.CarDistanceY != 0) { yStatus = 1; } } if(yStatus == 1) // 提速阶段 { if(xCarParam.CarDistanceY > 0) { Position_PID(&DistanceY,xCarParam.EncoderSumY,xCarParam.CarDistanceY); xCarParam.Speed_Y = DistanceY.result; if(xCarParam.Speed_Y <= uStartSpeed) { yStatus = 2; } else if (xCarParam.Speed_Y >= uStartSpeed + speed_up_cnt_y) { speed_up_cnt_y += increase_rat_y; xCarParam.Speed_Y = uStartSpeed + speed_up_cnt_y; } else { yStatus = 2; speed_up_cnt_y = 0; } } if(xCarParam.CarDistanceY < 0) { Position_PID(&DistanceY,xCarParam.EncoderSumY,xCarParam.CarDistanceY); xCarParam.Speed_Y = DistanceY.result; if(xCarParam.Speed_Y >= -uStartSpeed) { yStatus = 2; } else if (xCarParam.Speed_Y <= -uStartSpeed - speed_up_cnt_y) { speed_up_cnt_y += increase_rat_y; xCarParam.Speed_Y = -uStartSpeed - speed_up_cnt_y; } else { yStatus = 2; speed_up_cnt_y = 0; } } } if(yStatus == 2) { Position_PID(&DistanceY,xCarParam.EncoderSumY,xCarParam.CarDistanceY); xCarParam.Speed_Y = DistanceY.result; if(abs(xCarParam.EncoderSumY - xCarParam.CarDistanceY)<40||abs(xCarParam.Speed_Y)<30) { yStatus = 0; xCarParam.EncoderSumY = 0; xCarParam.CarDistanceY = 0; xCarParam.Speed_Y = 0; } } }
时间: 2024-04-18 12:23:36 浏览: 11
这段代码是用于控制小车在Y方向上的速度的函数。
函数中首先判断yStatus的值,如果为0,表示当前处于初始状态。在初始状态下,通过位置PID控制计算出Y方向上的速度,并将其赋值给xCarParam.Speed_Y。然后判断小车需要移动的距离是否为0,如果不为0,则将yStatus设置为1,表示进入提速阶段。
在提速阶段(yStatus为1),根据小车需要移动的距离的正负情况分别进行处理。如果CarDistanceY大于0,表示小车需要向正方向移动,继续使用位置PID控制计算出Y方向上的速度,并将其赋值给xCarParam.Speed_Y。然后判断当前速度是否小于等于起始速度uStartSpeed,如果是,则将yStatus设置为2,表示进入匀速阶段。如果当前速度大于等于起始速度加上speed_up_cnt_y,则表示需要加速,将加速度increase_rat_y添加到speed_up_cnt_y中,并将速度设置为起始速度加上speed_up_cnt_y。否则,将yStatus设置为2,同时将speed_up_cnt_y重置为0。
如果CarDistanceY小于0,表示小车需要向负方向移动,同样进行类似的处理。
在匀速阶段(yStatus为2),继续使用位置PID控制计算出Y方向上的速度,并将其赋值给xCarParam.Speed_Y。然后判断小车当前的位置和目标位置的差值是否小于40,或者当前速度的绝对值是否小于30。如果满足其中一个条件,则表示小车已经接近目标位置或者速度很小,将yStatus设置为0,同时将一些参数重置为0,以准备下一次移动。
这段代码实现了对小车在Y方向上速度的控制,包括初始阶段的加速、匀速阶段的保持速度以及接近目标位置时的减速停止。