The objective of this lab is to use all the things I have done so far to achieve fast stunts.
The task I choose is task B. It is quite simple to achieve the required stunt. I need to add the code to achieve a 180 degree turn when the car is close to the wall. It need to be done only once and use both PID controller for TOF sensor and orientation. Here is the code
if(distance1<500 && imu_pid_setpoint==true){
//map to -180 to 180
setPoint1_imu = fmod(yaw_imu + 180, 360);
if (setPoint1_imu > 180) {
setPoint1_imu -= 360;
}
imu_pid_setpoint=false;
}
// if the setpoint has set and the error exceeds the limit
if(imu_pid_setpoint==false && abs(setPoint1_imu-yaw_imu)>12){
input_PID_imu = yaw_imu;
PID_forimu(Kp_imu,Ki_imu,Kd_imu,Imax_imu,minspeed_imu,setPoint1_imu);
change_speed(output_PID_imu,-output_PID_imu);
}
// return back
if(abs(setPoint1_imu-yaw_imu)<=12 && imu_pid_setpoint==false){
change_speed(100,100);
}
Sometimes, the car need spin around several times to attain the correct angle. In order to sdeal with this problem, I set the largest PWM in the orientation PID to be 150 to solve this problem. Here are three videos.
In the beginning, the car sometimes need more time to rotate to the designed direction.
video 1
After adjustment, it can achieve the performed result.
video 2
video 3