為什么我程序在主函數里只運行了一次，當傳感器再次檢測到時，沒有任何反應 灰度傳感器的位置是 前排 5個構成了一排
以下是主函數和pid程序
int main(void)
{
delay_init(168); //初始化延時函數
uart_init(115200);//初始化串口波特率為115200
TIM3_PWM_Init(999,84-1); // PWM頻率=72M/((2*(arr+1)*(psc+1))
EXTIX_Init(); //分頻系數 84 / 84 =1Mhz 1Mhz/1000=1Khz。
motor_init();
while(1)
{
//read_sensor_value_1();
read_sensor_value_2();
calc_pid();
motor_control();
}
}
void read_sensor_value_2()
{
if(PDin(8) == 1)
{
error=2;
}
else if(PDin(9) == 1)
{
error=1;
}
else if(PDin(10) == 1)
{
forward_min();
}
else if(PDin(11) == 1)
{
error=-1;
}
else if (PDin(12) == 1)
{
error=-2;
}
else if ((PDin(12) == 0)|(PDin(11) == 0)|(PDin(10) == 0)|(PDin(9) == 0)|(PDin(8) == 0))
{
if(error==-2)
{
error=-3;
}
else
{
error=3;
}
}
//return 0;
}
float P=0,I=0,D=0,PID_value=0;
float previous_error = 0, previous_I = 0; //誤差值
float Kp=50,Ki=0,Kd=0;
void calc_pid()
{
P=error;
I=I+error;
D=error-previous_error;
PID_value = (Kp * P) + (Ki * I) + (Kd * D);
previous_error = error;
}
static float initial_motor_speed = 0; //初始速度
//限幅（-305，305）
void motor_control()
{
float left_motor_speed = initial_motor_speed - PID_value;
float right_motor_speed = initial_motor_speed + PID_value;
// if(left_motor_speed < -305){
// left_motor_speed = -305;
// }
// if(left_motor_speed > 305){
// left_motor_speed = 305;
// }
// if(right_motor_speed < -305){
// right_motor_speed = -305;
// }
// if(right_motor_speed > 305){
// right_motor_speed = 305;
// }
motorsWrite(left_motor_speed,right_motor_speed);
}
void motorsWrite(int speedL, int speedR)
{
if (speedR > 0)
{
TIM_SetCompare3(TIM3,350+speedR); //B0
TIM_SetCompare4(TIM3,0); // 右正轉
TIM_SetCompare3(TIM4,360); //B8
TIM_SetCompare4(TIM4,0); //B9
TIM_SetCompare1(TIM3,360); //A6
TIM_SetCompare2(TIM3,0); //A7
} else {
TIM_SetCompare3(TIM3,0); //B0
TIM_SetCompare4(TIM3,350+abs(speedR)); //倒轉
TIM_SetCompare3(TIM4,360); //B8
TIM_SetCompare4(TIM4,0); //B9
TIM_SetCompare1(TIM3,360); //A6
TIM_SetCompare2(TIM3,0); //A7
}
if (speedL > 0)
{
TIM_SetCompare1(TIM4,350+speedL);
TIM_SetCompare2(TIM4,0); // 左正轉
TIM_SetCompare3(TIM4,360); //B8
TIM_SetCompare4(TIM4,0); //B9
TIM_SetCompare1(TIM3,360); //A6
TIM_SetCompare2(TIM3,0); //A7
} else {
TIM_SetCompare1(TIM4,0);
TIM_SetCompare2(TIM4,350+abs(speedL));
TIM_SetCompare3(TIM4,360); //B8
TIM_SetCompare4(TIM4,0); //B9
TIM_SetCompare1(TIM3,360); //A6
TIM_SetCompare2(TIM3,0); //A7
}
}
|
