My partner Lizzie Manabat and I decided to work on the hardware option of the final project. This included taking the project 3 from where I left off, and adding the capabilities of follow-me 1D and 2D driving using both the LIDAR and ultrasonic sensor.
This project directly builds on and extends project 3, as we needed to successfully install 3 sensors and make sure they worked in the project 3. In the final project however, we needed to implement project 3’s code into the follow-me 1D and 2D.
I contributed to this project as the driver of the code. Lizzie was more of a navigator and tinkering with the ideas to implement our algorithm. We could say I worked more on the software and she worked more on the hardware, including the wiring and maintenance of the mechanical pieces. However, at the end of the day, we each did contribute to both hardware and software and got to understand how everything worked – from the start to the end.
On the way, we faced many obstacles, the biggest one of which being the WiringPi library being incompatible with other software that was installed on the Raspberry Pi, resulting in unexpected crashes at unexpected times. We solved the issue by manually ending the process by using the following commands:
ps -ef | grep follow
kill -9 <processnumber>
The first line of code above allows us to find every process including the keyword “follow”, as our project filename was follow-2D. The second line of code force ends the process, so that it is no longer running. After that, we go back to our directory, and rerun our program without any crashed (until it crashes again.) Unfortunately, this is only a temporary fix and the permanent fix lies in changing the library used/ using Pico instead of Raspberry Pi itself to control the sensors.
Here is out video of the working robot:
Here is our code for 1D:
/**
* File: 1D_control.cpp
*
* Controls the robot to maintain a given distance to the wall directly in
* front of it.
*
* This code uses Bang-Bang control or P-control to maintain the setpoint
* distance.
*/
#include <iostream>
#include <cmath>
#include <signal.h>
#include <follow_me/common/utils.h>
#include <mbot_bridge/robot.h>
#include <wiringPi.h>
#define TRIG 21
#define ECH 22
int getCM() {
//Send trig pulse
digitalWrite(TRIG, HIGH);
delayMicroseconds(20);
digitalWrite(TRIG, LOW);
//Wait for echo start
while(digitalRead(ECH) == LOW);
std:: cout<< "L \n";
//Wait for echo end
long startTime = micros();
while(digitalRead(ECH) == HIGH);
std:: cout<< "H \n";
long travelTime = micros() - startTime;
//Get distance in cm
int distance = travelTime / 58;
return distance;
}
bool ctrl_c_pressed;
void ctrlc(int)
{
ctrl_c_pressed = true;
}
int main(int argc, const char *argv[])
{
signal(SIGINT, ctrlc);
signal(SIGTERM, ctrlc);
wiringPiSetup();
pinMode(TRIG, OUTPUT);
pinMode(ECH, INPUT);
//TRIG pin must start LOW
digitalWrite(TRIG, LOW);
delay(30);
// Initialize the robot.
mbot_bridge::MBot robot;
// We will store the Lidar scan data in these vectors.
std::vector<float> ranges;
std::vector<float> thetas;
// *** Task 1: Adjust these values appropriately ***
float setpoint = 30; // The goal distance from the wall in meters
float error = 5;
float velocity= 50;
// *** End student code *** //
while (true) {
// This function gets the Lidar scan data.
robot.readLidarScan(ranges, thetas);
// Get the distance to the wall.
float dist_to_wall = findFwdDist(ranges, thetas);
// *** Task 2: Implement the Follow Me controller *** //
if (dist_to_wall < 0) continue;
int distance1 = getCM();
std:: cout << "echo: " << distance1 << "\n";
std:: cout << "lidar: " << dist_to_wall << "\n";
if ( distance1 >= 35 && dist_to_wall >=0.35) robot.drive (0.3,0,0); // if the distance sensor and lidar both show a value bigger than 35cm, then drive forward.
else if (dist_to_wall > 0.23 && dist_to_wall <0.35 || distance1 > 20 && distance1 <35 ) robot.drive (0,0,0); // if just distance sensor or just the lidar show a value lower than threshold but higher than the low limit, then stop.
else if (dist_to_wall <= 0.23 || distance1 <= 20 ) robot.drive (-0.3,0,0); // if too close to the mbot, move back. Either the lidar or the distance sensor.
// if ((dist_to_wall >=0.25) && (dist_to_wall <=0.35)) robot.stop();
else robot.drive(0,0,0); //else stop the robot. not necessary but it's still there.
// *** End Student Code *** //
if (ctrl_c_pressed)
break;
}
// Stop the robot.
robot.stop();
return 0;
}
And here is our code for 2D:
/**
* File: 2D_control_nearest.cpp
*
* Controls the robot to maintain a given distance to the nearest wall.
*
* This code finds the distance to the nearest wall in the Lidar scan. It
* applies a control to the robot in the direction of the wall using the angle
* to the scan.
*/
#include <iostream>
#include <cmath>
#include <signal.h>
#include <string>
#include <follow_me/common/utils.h>
#include <mbot_bridge/robot.h>
#include <wiringPi.h>
#define TRIG 21
#define ECH 22
#define TRIG2 2
#define ECHO2 3
#define TRIG3 12
#define ECHO3 13
using namespace std;
bool ctrl_c_pressed;
void ctrlc(int)
{
ctrl_c_pressed = true;
}
int getCM() {
//Send trig pulse
digitalWrite(TRIG, HIGH);
delayMicroseconds(20);
digitalWrite(TRIG, LOW);
//Wait for echo start
while(digitalRead(ECH) == LOW);
std:: cout<< "L \n";
//Wait for echo end
long startTime = micros();
while(digitalRead(ECH) == HIGH);
std:: cout<< "H \n";
long travelTime = micros() - startTime;
//Get distance in cm
int distance = travelTime / 58;
return distance;
}
int getCM2() {
//Send trig pulse
digitalWrite(TRIG2, HIGH);
delayMicroseconds(20);
digitalWrite(TRIG2, LOW);
//Wait for echo start
while(digitalRead(ECHO2) == LOW);
//Wait for echo end
long startTime = micros();
while(digitalRead(ECHO2) == HIGH);
long travelTime = micros() - startTime;
//Get distance in cm
int distance2 = travelTime / 58;
return distance2;
}
int getCM3() {
//Send trig pulse
digitalWrite(TRIG3, HIGH);
delayMicroseconds(20);
digitalWrite(TRIG3, LOW);
//Wait for echo start
while(digitalRead(ECHO3) == LOW);
//Wait for echo end
long startTime = micros();
while(digitalRead(ECHO3) == HIGH);
long travelTime = micros() - startTime;
//Get distance in cm
int distance3 = travelTime / 58;
return distance3;
}
/**
* @brief findMinDist takes an vector of ranges, and returns the
* index of the smallest range in the vector
*
* @param ranges
* @return int
*/
int findMinDist(const std::vector<float>& ranges)
{
// *** Task 2: Implement findMinDist logic *** //
// TODO: Add logic so that findMinDist returns the correct index.
int index = 0;
for (float min_idx = 1; min_idx < ranges.size(); min_idx++)
{
if (ranges[min_idx] != 0){
if (ranges[min_idx] < ranges[index]){
index = min_idx;
}
}
}
return index;
// HINT: Remember to ignore any ranges that are zero!
}
int main(int argc, const char *argv[])
{
signal(SIGINT, ctrlc);
signal(SIGTERM, ctrlc);
//initialize all 3 sensors
wiringPiSetup();
pinMode(TRIG, OUTPUT);
pinMode(ECH, INPUT);
pinMode(TRIG2, OUTPUT);
pinMode(ECHO2, INPUT);
pinMode(TRIG3, OUTPUT);
pinMode(ECHO3, INPUT);
//TRIG pin must start LOW
digitalWrite(TRIG, LOW);
digitalWrite(TRIG2, LOW);
digitalWrite(TRIG3, LOW);
delay(30);
// Initialize the robot.
mbot_bridge::MBot robot;
// We will store the Lidar scan data in these vectors.
std::vector<float> ranges;
std::vector<float> thetas;
// *** Task 1: Adjust these values appropriately ***
// *** End student code *** //
while (true) {
// This function gets the Lidar scan data.
robot.readLidarScan(ranges, thetas);
// Get the distance to the wall.
float min_idx = findMinDist(ranges);
float dist_to_wall = ranges[min_idx];
float angle_to_wall = thetas[min_idx];
int dist1 = getCM();
int dist2 = getCM2();
int dist3 = getCM3();
// *** Task 3: Implement the 2D Follow Me controller ***
// Hint: Look at your code from follow_1D
// TODO: create a shortestdistance variable and iterate through the three sensors and assign the smallest sensor value data to shortestdistance variable, and then compare that variable to to each sensor data to find the sensor that's shortest.
int shortestdistance = 1000000;
if (dist1< shortestdistance){
shortestdistance = dist1;
}
if (dist2< shortestdistance){
shortestdistance = dist2;
}
if (dist3< shortestdistance){
shortestdistance = dist3;
}
if (shortestdistance < dist_to_wall * 100 + 6.5){ //shortestdistance is in cm while dist_to_wall is in meters.
cout<< "dist1: "<< dist1 <<endl;
cout<< "dist2: "<< dist2 <<endl;
cout<< "dist3: "<< dist3 <<endl;
cout<< "dist_to_wall: "<< dist_to_wall <<endl;
float setpoint = 30; // The goal distance from the wall in meters
float margin = 5;
float speed= 0.3;
if (shortestdistance == dist1){
cout<<"shortestdistance: "<< dist1<< endl;
angle_to_wall = 0;
}
else if (shortestdistance == dist2){
cout<<"shortestdistance: "<< dist2<< endl;
angle_to_wall = 120;
}
else if (shortestdistance == dist3){
cout<<"shortestdistance: "<< dist3<< endl;
angle_to_wall = 240;
}
float vx = speed*(cos(angle_to_wall));
float vy = speed*(sin(angle_to_wall));
cout << "sensor vx " << vx << endl;
cout << "sensor vy " << vy << endl;
if (shortestdistance > setpoint+ margin) {
robot.drive(vx, vy, 0);
}
else if (shortestdistance < setpoint - margin){
robot.drive(-vx,-vy,0);
}
else {
robot.drive(0,0,0);
}
}
else{
cout<<"shortestdistance: "<< dist_to_wall<< endl;
float setpoint = 0.3; // The goal distance from the wall in meters
float margin = 0.05;
float speed = 0.3;
float angle_to_wall = thetas[min_idx];
float vx = speed*(cos(angle_to_wall));
float vy = speed*(sin(angle_to_wall));
cout << "lidar vx " << vx << endl;
cout << "lidar vy " << vy << endl;
if (dist_to_wall > setpoint+ margin) {
robot.drive(vx, vy, 0);
}
else if (dist_to_wall < setpoint - margin){
robot.drive(-vx,-vy,0);
}
else {
robot.drive(0,0,0);
}
}
// Hint: When you compute the velocity command, you might find the functions
// sin(value) and cos(value) helpful!
// *** End Student Code ***
if (ctrl_c_pressed) break;
}
// Stop the robot.
robot.stop();
return 0;
}
