Home - nscu225/BAE305Lab9 GitHub Wiki
By: Natalie Cupples
In Lab 9 (Take Control: The PID Feedback Controller), we worked on setting up a PID control system to help a robot stay a certain distance away from a wall. We used an ultrasonic sensor to measure the distance and controlled the robot’s motors using an Arduino. First, we added the PID_v2 library to the code and set up the set point, measurement, and output. After that, we programmed the robot to move forward or backward based on how far it was from the wall. Later, we modified the robot so it could follow along a wall by adjusting the left and right motor speeds separately. Although we had to tune the Kp, Ki, and Kd values a few times to get it working smoothly, each part of the lab was successful. Overall, this lab helped us understand how PID controllers work and how to tune them to make a system more stable.
-
A Computer running Arduino IDE and Chrome Browser
-
SparkFun Inventor’s kit
• RedBoard
• Ultrasonic sensor
• Two motors
• Motor Driver
• Battery Pack
Figure 1; Arduino IDE software on computer
Figure 2; SparkFun Inventor’s kit
- In the Arduino IDE toolbox, install the PID_V2 library by Brett Beauregard.
- Open and modify the robot sketch that was written in Lab 6.
- At the beginning of the code, add the line #include <PID_v2.h> to connect the newly added library
- Use type double to declare the set point, measurement, output, Kp, Ki, and Kd. Give the set point a value between 10 and 20 and the Kp a value between 1 and 5. The rest should be 0.
- Include the line myPID(&measurement, &output, &setpoint, Kp, Ki, Kd, DIRECT); to create the PID instance.
- Include the lines myPID.SetTunings(Kp, Ki, Kd); and myPID.SetMode(AUTOMATIC); to initialize the PID.
- Include the line myPID.Compute(); in the loop function to run the PID after calculating the distance.
- Write the setpoint, measurement, and output values to the serial port to verify the operation.
- See our final code in Figure 4 in Results*
- Add a function to the code that tells the robot to move forward or backward depend ending on a measured distance.
- Use the function (map(variable, minValue, maxValue, newminValue, newmaxValue)) and assign the motor speed a value between -255 and 255.
- Tune the Kp, Ki, and Kd values until the robot is performing correctly.
- See our final code in Figure 5 in Results*
- Modify the code so the robot maintains the same distance from the wall by moving forwards or backwards. This modification will change the PID controller and utilize the ultrasonic sensor.
- Add a function that allows the left and right wheels move at different speeds.
- Modify the map function using basic arithmetic to change the right and left speeds depending on the output of the PID.
- Tune the PID controller like in step 2.1
- See our final code in Figure 6 in Results*
Figure 3: Our robot car
Figure 4: Final code for Part 1 of the lab
#include <SoftwareSerial.h>
#include <PID_v2.h>
int counter = 0;
SoftwareSerial mySerial(2, 3); // HC-05 Tx connected to Arduino #2 & HC-05 Rx to Arduino #3
const byte numChars = 16;
char receivedChars[numChars]; // an array to store the received data
char tempChars[numChars];
char botDir[numChars] = {0}; // char type variable for the direction of the robot
int botSpeed = 0; //stores the speed of the whole robot
boolean newData = false;
//the right motor will be controlled by the motor A pins on the motor driver
const int AIN1 = 13; //control pin 1 on the motor driver for the right motor
const int AIN2 = 12; //control pin 2 on the motor driver for the right motor
const int PWMA = 11; //speed control pin on the motor driver for the right motor
//the left motor will be controlled by the motor B pins on the motor driver
const int PWMB = 10; //speed control pin on the motor driver for the left motor
const int BIN2 = 9; //control pin 2 on the motor driver for the left motor
const int BIN1 = 8; //control pin 1 on the motor driver for the left motor
const int trigPin = 6; //trigger pin for distance snesor
const int echoPin = 7; //echo pin for distance sensor
String botDirection; //the direction that the robot will drive in (this change which direction the two motors spin in)
String motorSpeedStr;
int motorSpeed; //speed integer for the motors
float duration, distance; //duration and distance for the distance sensor
double MEAS = 0;
double OUT = 0;
double setpoint = 15;
double Kp = 2;
double Ki = 2;
double Kd = 1;
PID myPID(&MEAS, &OUT, &setpoint, Kp, Ki, Kd, DIRECT);
/********************************************************************************/
void setup()
{
mySerial.begin(9600); //Default Baud Rate for software serial communications
//set the motor control pins as outputs
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
//set the distance sensor trigger pin as output and the echo pin as input
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
myPID.SetTunings(Kp, Ki, Kd);
myPID.SetMode(AUTOMATIC);
Serial.begin(9600); //begin serial communication with the computer
//prompt the user to enter a command
Serial.println("Enter a direction followed by speed.");
Serial.println("f = forward, b = backward, r = turn right, l = turn left, s = stop");
Serial.println("Example command: f 50 or s 0");
}
/********************************************************************************/
void loop()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
delay(2);
duration = pulseIn(echoPin, HIGH);
distance = (duration*340/10000)/2; // Units are cm
MEAS = distance;
myPID.Compute();
Serial.print(setpoint);
Serial.print(",");
Serial.print(MEAS);
Serial.print(",");
Serial.println(OUT);
moveWithPID(OUT);
counter = counter + 1;
if (counter%200 == 0)
{
mySerial.print("Distance: ");
mySerial.println(distance);
Serial.print("Distance: ");
Serial.println(distance);
delayMicroseconds(100);
}
if (Serial.available() > 0) //if the user has sent a command to the RedBoard
{
botDirection = Serial.readStringUntil(' '); //read the characters in the command until you reach the first space
motorSpeedStr = Serial.readStringUntil('\n'); //read the characters in the command until you reach the second space
motorSpeed = motorSpeedStr.toInt();
Serial.print(botDirection);
Serial.print(" ");
Serial.println(motorSpeedStr);
}
recvWithEndMarker();
if (newData == true)
{
strcpy(tempChars, receivedChars);
parseData();
botDirection = botDir;
motorSpeed = botSpeed;
newData = false;
Serial.println(botDirection);
Serial.println(motorSpeed);
}
}
/********************************************************************************/
void rightMotor(int motorSpeed) //function for driving the right motor
{
if (motorSpeed > 0) //if the motor should drive forward (positive speed)
{
digitalWrite(AIN1, HIGH); //set pin 1 to high
digitalWrite(AIN2, LOW); //set pin 2 to low
}
else if (motorSpeed < 0) //if the motor should drive backward (negative speed)
{
digitalWrite(AIN1, LOW); //set pin 1 to low
digitalWrite(AIN2, HIGH); //set pin 2 to high
}
else //if the motor should stop
{
digitalWrite(AIN1, LOW); //set pin 1 to low
digitalWrite(AIN2, LOW); //set pin 2 to low
}
analogWrite(PWMA, abs(motorSpeed)); //now that the motor direction is set, drive it at the entered speed
}
/********************************************************************************/
void leftMotor(int motorSpeed) //function for driving the left motor
{
if (motorSpeed > 0) //if the motor should drive forward (positive speed)
{
digitalWrite(BIN1, HIGH); //set pin 1 to high
digitalWrite(BIN2, LOW); //set pin 2 to low
}
else if (motorSpeed < 0) //if the motor should drive backward (negative speed)
{
digitalWrite(BIN1, LOW); //set pin 1 to low
digitalWrite(BIN2, HIGH); //set pin 2 to high
}
else //if the motor should stop
{
digitalWrite(BIN1, LOW); //set pin 1 to low
digitalWrite(BIN2, LOW); //set pin 2 to low
}
analogWrite(PWMB, abs(motorSpeed)); //now that the motor direction is set, drive it at the entered speed
}
/********************************************************************************/
void recvWithEndMarker() {
/*Taken from https://forum.arduino.cc/t/serial-input-basics-updated/38200. Posted by Robin2*/
static byte ndx = 0;
char endMarker = '\n';
char rc;
while (mySerial.available() > 0 && newData == false){
rc = mySerial.read();
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars){
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}
/*****************************************************************************************/
void parseData() { // split the data into its parts
/*Taken from https://forum.arduino.cc/t/serial-input-basics-updated/38200. Posted by Robin2*/
char * strtokIndx; // this is used by strtok() as an index
strtokIndx = strtok(tempChars," "); // get the first part - the string
strcpy(botDir, strtokIndx); // copy it to messageFromPC
strtokIndx = strtok(NULL, " "); // this continues where the previous call left off
//strcpy(botSpeed, strtokIndx); // Use this line for sending speed as text
botSpeed = atoi(strtokIndx); // Use this line for sending sepeed as an integer
}Figure 5: The additions to the original code that setup Part 2 of this lab
void moveWithPID(double pidOutput)
{
int motorSpeed = map(pidOutput, 0, 255, -255, 255);
rightMotor(motorSpeed);
leftMotor(motorSpeed);
}Figure 6: The additions to the original code that setup Part 3 of this lab
void moveWithPID(double pidOutput)
{
int rightmotorSpeed = map(pidOutput, 0, 255, -100, 100);
int leftmotorSpeed = map(pidOutput, 0, 255, -100, 100);
if (setpoint > MEAS) {
rightMotor(rightmotorSpeed);
leftMotor(leftmotorSpeed - 10);
}
else if (setpoint < MEAS) {
rightMotor(-rightmotorSpeed);
leftMotor(-leftmotorSpeed + 10);
}
else {
rightMotor(0);
leftMotor(0);
}
}Although there weren't any discussion questions to answer throughout the lab, I'll share my thoughts on things that went right and wrong while completing this assignment. To start, the program in Part 1 wasn't writing the correct responses. Instead of giving three numerical values for measurement, output, and set point, it instead read a number, 0, then another number. I think that this error came from not establishing MEAS = distance; early enough in my code. However, after moving on from this part, my robot did a good job following the new functions and responded well to objects being in front of it.
In this lab, I learned how to use a PID controller within the Arduino software to control the movement of a robot car. This was done by importing a library into previously written code and running it with an ultrasonic sensor. I learned how to tune the Kp, Ki, and Kd values to get the robot moving smoothly and responding correctly. I also learned how to use the map function to scale the PID output, allowing the robot to adjust its movements based on its proximity to a wall. Overall, this lab helped me better understand how feedback control systems work in real-world applications.