PreLab
Orientation control: Create a PID controller that allows your robot to do on-axis turns in small, accurate increments.
Firstly, we need a program to enable the car to rotate for a full round and then record the distance TOF data at different angle. I wrote the code below to finish this task.
case Mapping:
{
float kp,ki,kd;
float LastPID = 0.0;
float prevError=0.0,error=0.0,integral=0.0;
float P,I,D,PID;
float current_time=0.0,current_yaw;
//get the pid parameter from the computer
success = robot_cmd.get_next_value(kp);
if (!success)
return;
success = robot_cmd.get_next_value(ki);
if (!success)
return;
success = robot_cmd.get_next_value(kd);
if (!success)
return;
Serial.print("Get PID parameter: ");
Serial.print(kp);
Serial.print(", ");
Serial.print(ki);
Serial.print(", ");
Serial.println(kd);
int start_time = (int)millis();
int int_last_time = (int)millis();
int tofcnt = 0;
Serial.println("Start Running! ");
distanceSensor1.startRanging(); //Write configuration bytes to initiate measurement
yaw = 0.0;
//initialize the target_yaw
last_time = millis();
orientation_yaw(); //get the current yaw
float target_yaw = yaw;
bool flag= false;
while ( ((int)millis()-start_time) < 11000 ) //set to collect data for 10s
{
orientation_yaw(); //get the current yaw
// adding 90 degrees to the set point after 5 seconds
if(((int)millis()-int_last_time)>=200)
{
if (distanceSensor1.checkForDataReady())
{
if(distanceSensor1.getDistance()< 1)
{
continue;
}
mapping_distance[tofcnt] = distanceSensor1.getDistance(); //Get the result of the measurement from the sensor
mapping_yaw[tofcnt] = yaw;
Serial.print("current yaw: ");
Serial.print(yaw);
Serial.print(" current distance: ");
Serial.println(mapping_distance[tofcnt]);
tofcnt++;
distanceSensor1.clearInterrupt();
//already collected distance data, increase target yaw
target_yaw = target_yaw + 10;
if(target_yaw>360)
{
target_yaw = 360;
}
integral=0;
int_last_time = (int)millis();
}
else{continue;}
}
current_time = (int)millis();
//Serial.println(yaw);
current_yaw = yaw;
// store the data
// tof_time[tofcnt] = (float)millis()/1000;
//curyaw[tofcnt] = yaw;
//update the last1 and last2 distance and last time
error = current_yaw-target_yaw; //should be larger than 0 at the beginning
P = kp * error;
if((error >= 0 and LastPID < = 0) or (error < 0 and LastPID > 0))
{
integral += error;
integral = min(max(integral,-50),50); //wind-up protection
}
I = ki * integral;
D = kd * (error - prevError);
prevError = error;
PID = P + I + D;
//when yaw decrease, turn left, error < 0
//when yaw increase, turn right, error > 0
PID = min(250,max(PID,-250));
if(PID>=0){turnright((PID/250)*180+70);//curspeed[tofcnt-1] = (PID/250)*200+50;
}
else{turnleft((-PID/250)*180+70);//curspeed[tofcnt-1] = -((-PID/250)*200+50);
}
LastPID = PID;
}
forward(0);
//send the data
//only 929 for 10s count
// Serial.print(tofcnt);
for(int i=0;i < tofcnt;i++)
{
tx_estring_value.clear();
tx_estring_value.append("distance: ");
tx_estring_value.append(mapping_distance[i]);
tx_estring_value.append(" yaw: ");
tx_estring_value.append(mapping_yaw[i]);
tx_characteristic_string.writeValue(tx_estring_value.c_str());
}
}
break;
The most important code which controls the orientation is shown below. Everytime after spinning for more than 0.2 seconds, we believe the car has reached the target angle which is 10 degrees larger than the previous step. Then we record the distance and yaw values if the current TOF data is available, and then increase the target yaw by 10 degrees meanwhile renew the integral part of the PID. Also we set a upper limit of the target yaw to be 360 degree so that the car could turn exactly a round.
if(((int)millis()-int_last_time)>=200)
{
if (distanceSensor1.checkForDataReady())
{
if(distanceSensor1.getDistance() < 1)
{
continue;
}
mapping_distance[tofcnt] = distanceSensor1.getDistance(); //Get the result of the measurement from the sensor
mapping_yaw[tofcnt] = yaw;
Serial.print("current yaw: ");
Serial.print(yaw);
Serial.print(" current distance: ");
Serial.println(mapping_distance[tofcnt]);
tofcnt++;
distanceSensor1.clearInterrupt();
//already collected distance data, increase target yaw
target_yaw = target_yaw + 10;
if(target_yaw>360)
{
target_yaw = 360;
}
integral=0;
int_last_time = (int)millis();
}
else{continue;}
}
The result of the PID controller is shown in the video below:
We can observe that the car can rotate smoothly, even if there is some drift but the car could still remain in the same tile, the TA said it is total acceptable.
To know whether the data is correctly recorded, we printed out the data received on our computer, shown in the figure below:
As shown in the figure above, the real yaw is increased roughly 10 degrees each time and stop at 360 degrees, which means our program is effective.
