Skip to content

Instantly share code, notes, and snippets.

@jgkawell
Created February 1, 2020 16:30
Show Gist options
  • Save jgkawell/e4f91f5dc99e5984bf0b60be24957587 to your computer and use it in GitHub Desktop.
Save jgkawell/e4f91f5dc99e5984bf0b60be24957587 to your computer and use it in GitHub Desktop.
Code for the Autonomous Rover as shown in this video: https://www.youtube.com/watch?v=7baK2on4ls4

Information Processing Overview

Final Project Report

Physics Seminar Spring 2017

Roman Travis, Jack Kawell, Christina Van Wingerden, Tristan Widra, and Chris Thrasher

The overall goal for the Physics Seminar this semester has been to produce an autonomous rover capable of moving throughout a “simple” environment without hitting any obstacles. This complex idea was made more manageable by dividing the responsibilities into three groups: Data Acquisition, Information Processing, and Actuation. Our team, Information Processing, was responsible for receiving data from the acquisition team, making decisions, and relaying those decisions to the actuation team. As the “middle-man” of the project, our actual responsibilities were more fluid since we worked alongside the other two teams to better facilitate inter-process communication. This report will catalog what we did and what we learned as a result.

The first hurdle we addressed was connecting the lidar to the Arduino Uno. The lidar unit connected to the Uno through a 5V power source, ground, and two information wires. Because we were using I2C as opposed to PWM for communication, we connected the information wires into the SDA and SCL pins. This allowed us to completely power and receive data from the lidar using only the Uno. We were also able to read the data using the serial monitor in the Arduino IDE. This was displayed as either an auto-scaling graph or as a series of distances in cm. The most helpful part of this section of the project was being able to use premade libraries specifically for connecting our lidar unit to an Arduino. All we had to do to measure a distance was call the distance() method. Then to view the distance, we only had to call a serial.print() method which displayed the distance on the computer’s serial monitor.

With the lidar functioning correctly, it was time to move on to the servo. This was also fairly simple to use. The IDE came with a premade servo library, so that all we had to do was tell the servo a position to turn to in degrees. This gave us simple and precise control of the servo’s position in a format that was easy to use in the rest of the code. One problem we did discover was the need for a small pause in the code between each order given to the Servo. If we gave consecutive orders too quickly, the servo would not make it all the way to the first position. This problem was easy to fix with a small (15ms) wait in the code, and-for the most part-the servo alone didn’t give us too much trouble.

With the individual components working off of prewritten example code, it was time to operate the lidar and servo together. This required the creation of another program. Essentially, we created two for loops using the counter variable i as the servo angle. The first loop took the servo from 0 to 180 degrees. The second loop took the servo back from 180 to 0 degrees. The values of the distances at each angle were displayed on the serial monitor. Up to this point, the project worked very smoothly.

The problem arose when we tried to save the distances in an array. Instead of storing distances in centimeters, we stored them as 1’s and 0’s depending on their size. We compared the distance to a predetermined “safety distance” – usually 50cm. If a distance was greater than 50cm, it was assigned a 0. If the distance was less than 50cm, it was assigned a 1. The goal of this was to simplify the decisions (and therefore the calculations) as well as to lighten the load on the Uno. Sadly, the switch to binary did not fully accomplish this because the array in which we stored the 1’s and 0’s was still a full int array and so was taking up most of the memory on the Uno.

Before moving to the solution for this problem, let’s pause and take a minute to understand the flow of our code. Essentially, the rover sees the world as a 181 length, integer array (the 181st position is to account for both 0° and 180°). Each position is populated with either a 1 or a 0. A 1 indicates the presence of an object within the safety distance – possibly necessitating some action – while a 0 indicates the absence of any close obstacle. When the rover completes a forward movement, it takes a 180° scan of its forward environment, follows the following logic:

  1. It checks the center of the array (the actual size of this check is around 60°) for any 1’s.
    • If the section is clear, it proceeds forward for 1 second.
    • If the section is at all obstructed, it moves to the next set of logic.
  2. With an object in front of it, it then scans through the array to find the largest series of uninterrupted 0’s.
    • If the largest gap is large enough for the rover to fit through, it turns to the center of the gap, scans again, and returns to step 1 of the logic.
    • If the largest gap is too small but contains one of the edge points (0° or 180°), it turns to that outermost point, scans again, and repeats step 1.
    • If the largest gap is too small and does not contain an edge, the rover backs up for .75 seconds, scans again, and reverts to step 1.

One note, the lidar occasionally throws up false “1’s” depending on the size of the safety distance. This made it turn when its path was unobstructed. To address this, we only consider a 1 to be an object if it is next to at least four other consecutive 1’s. Most objects we encounter are greater than 5° wide by the time they enter the rover’s safety distance, and false 1’s are avoided.

We now return to the narrative. When we realized that the Uno didn’t have enough RAM to handle the array and other processes, we ordered the Arduino Due, which hadconsiderably more memory and power. This solved the problem of insufficient memory on the chip. While the Due did use 3.3V output pins instead of the Uno’s 5V pins, thelidar and servo were still able to operate (this was because the 3.3V of the Due matched the internal operating voltage of the lidar). When tested separately, both the servoand the lidar worked with the Due. However, when operated simultaneously, the servo and lidar would work for only a few sweeps –even correctly diagnosing objects –but aknack error would inevitably come up and the process would stop working. Measurements with an oscilloscope revealed that the 5V supply voltage for the lidar was dropping low but the knack error would still pop up after a few scans even with a steady supply. We solved this by switching to an Arduino Mega –a unit of similar processing power andmemory to the Due but with 5V as its internal voltage. This time the unit worked without producing the knack error. Better still, the Mega’s footprint matched the Due’s sothe motor shields and the mounted plastic holder still fit.

At this point, the rover was fully built and testing could begin. To control the motors, we sent a time, an intensity, and a direction to methods created by the Actuationteam. Since our earlier code specified each turn in degrees, we measured the rover’s average turn rate and used that to convert from degrees to time. We also measured itsforward speed at several intensities to find optimal travel speeds and times. With these conversions in place, a few more hours of tinkering had the rover successfullydriving around an area and dodging obstacles.

Overall we learned several skills and general project lessons. First, we learned how to use an Arduino board. On the coding side, we learned many commands and the basicstructure of the Arduino language. For example, Arduino code contains two main sections: the initializing section and the main loop. Connections to devices must be setup inthe initial section, while the next loop handles the meat of the code. More importantly, we learned how to use the prewritten examples as the foundational code to write ourmore complex algorithms. This was key to learning how to interact with the servo and the lidar. By copying portions of the demos, we were able to piece together workingprograms. It was surprising just how much these bits of code and some basic coding experience were able to accomplish.

A second broader lesson was how to take an abstract logical model and apply it to a physical process. We started early in the semester on chalk boards diagramming ways therover could “understand” and respond to its surroundings which we translated into Arduino code. Then piece-by-piece, we interfaced this code with the servo, lidar, andmotors. This was by far the most difficult part of the project. Going from logic to reality required constant troubleshooting, but after solving enough problems we produceda functional design. It was very gratifying to see the physical machine following the same reasoning that we wrote on the chalkboard months earlier.

The last skill we learned is how to work in a large segmented team on a single project. By necessity, our futures as engineers, scientists, and doctors will be characterizedby working in small teams as part of a larger group on complex problems. As we learned this semester, success requires constant communication between groups. For us, thistook the form of weekly meetings in class, spontaneous hallway discussions, and planned brainstorming sessions. It was very important that we looked up from our own sectionof the project to make sure we were fitting into the other pieces of the overall project. For instance, information processing may have known that differential steeringwould make the coding easier, but we had to actually relay that idea to actuation for that to happen.

Over the semester, we produced a rover capable of avoiding most large objects and meandering around a room. Our team learned how to code in Arduino and produced a programcapable of making decisions for the rover. We also interfaced the lidar and servo with the Arduino using online connection guides and premade libraries. We learned basictroubleshooting skills with small electronics and how to better work in a large team. Overall, the semester was a great learning experience which we will build on in the future.

//Initialize the Lidar
#include <Wire.h>
#include <LIDARLite.h>
LIDARLite myLidarLite;
//Initialize the Servo
#include <Servo.h>
Servo myservo; // create servo object to control a servo
// twelve servo objects can be created on most boards
//Pin declaration for 6 wheels
// Front
//
// 0 []| |[] 3
// 1 []| |[] 4
// 2 []| |[] 5
//
// Back
const uint8_t pwmPins[] = {3,5,6,9,10,11};
const uint8_t dirPins[] = {2,4,7,8,12,13};
const uint8_t velocity = 100;
const int duration = 1000;
//Declare global variables for program
int degreesToSweep = 180; //the number of degrees to sweep (we can alter this)
int obstacleArray[181]; //the array going from left to right
int i = 0; //the current array position
int safetyDistance = 75; //distance before recognizing an object is present (we can alter this)
int neededGap = 50;
//***************************************************************************
//The setup method that initializes all the sensors and motors
void setup() {
Serial.begin(9600); // Initialize serial connection to display distance readings
myLidarLite.begin(0, true); // Set configuration to default and I2C to 400 kHz
myLidarLite.configure(0); // Change this number to try out alternate configurations
myservo.attach(23); // attaches the servo on pin 9 to the servo object
//Sets all the pulse width modulation and direction pins to digital outputs
for(int i = 0; i < 6; i++) {
pinMode(pwmPins[i], OUTPUT);
pinMode(dirPins[i], OUTPUT);
}
}
//***************************************************************************
//The main loop method that sweeps the lidar back and forth and calls the turning method
void loop() {
Serial.print("CW :"); //for loop to go from left to right
for (i = 0; i <= degreesToSweep; i += 1) {
sweepMethod();
}
//decide which way to turn
turningMethod();
//delay(1000);
Serial.print("CCW:"); //for loop to go from right to left
for (i = degreesToSweep; i >= 0; i -= 1) {
sweepMethod();
}
//decide which way to turn
turningMethod();
//delay(1000);
}//end of main loop
//***************************************************************************
//A method that fills the arrays with 0s (if nothing is there)
//and 1s (if something is there)
void sweepMethod() {
//Tell the sero to move to another position
myservo.write(i);
int currentDistance = 100;
//Find and store the current distance
if (i % 90 == 0)
currentDistance = myLidarLite.distance();
else
currentDistance = myLidarLite.distance(false);
//Save the distance as either 1 (something is close) or 0 (nothing is close)
if (currentDistance < safetyDistance)
obstacleArray[i] = 1;
else
obstacleArray[i] = 0;
Serial.print(" ");
Serial.print(obstacleArray[i]);
delay(5);// waits for 15ms so the servo can get to pos before moving on for loop
}//end of sweepMethod
//***************************************************************************
//A method that gives turn commands
void turningMethod()
{
//Find the direction to head in
int degreeToTurn = arrayAnalysisMethod();
Serial.println(degreeToTurn);
if(degreeToTurn != -1){
int turningAngle = 0;
if(degreeToTurn < degreesToSweep/2)
turningAngle = (degreesToSweep/2) - degreeToTurn;
else if(degreeToTurn > degreesToSweep/2)
turningAngle = degreeToTurn - (degreesToSweep/2);
//Declare variables that control speed and call method to find time of turning
int timeToTurn = howLongToTurn(turningAngle);
int timeForward = 1000;
int turnVelocity = 100;
int forwardVelocity = 100;
if(degreeToTurn == degreesToSweep/2){
Serial.println("Just going straight...");
Serial.println(turningAngle);
//go forward
forward(forwardVelocity, timeForward);
}
//If the the degreeToTurn is less than the midpoint, turn left
//If it is more than the midpoint, turn right
if(degreeToTurn < degreesToSweep/2){
turnLeft(turnVelocity, timeToTurn);
Serial.print("Turning to the left...");
Serial.print(turningAngle);
Serial.println("...degrees");
//go forward
//forward(forwardVelocity, timeForward);
}else if(degreeToTurn > degreesToSweep/2){
turnRight(turnVelocity, timeToTurn);
Serial.print("Turning to the right...");
Serial.print(turningAngle);
Serial.println("...degrees");
//go forward
//forward(forwardVelocity, timeForward);
}
}else{
Serial.println("No opening big enough. Reversing...");
reverse(100,750);
}
}//end of turningMethod
//***************************************************************************
//A method that computes the amount of time to turn for a specific angle
int howLongToTurn(int degreeToTurn)
{
int timePerDeg = 35;//45ms
int timeToTurn = degreeToTurn * timePerDeg;
return timeToTurn;
}
//***************************************************************************
//A method that decides where to turn
int arrayAnalysisMethod()
{
//Variables for finding widest opening
int widthOfWidestGap = -1;
int startOfWidestGap = -1;
int sizeOfCurrentGap = -1;
int startOfCurrentGap = -1;
int sizeOfCurrentObject = -1;
//The following lines check to see if there is anything within a 46deg arc in front of the robot
//If there isn't anything there, it goes straight
boolean goStraight = true;
for(int j = degreesToSweep/2 - 30; j <= degreesToSweep/2 + 30; j += 1) {
if(obstacleArray[j] == 0) {
sizeOfCurrentObject = 0;
} else if(obstacleArray[j] == 1) {
sizeOfCurrentObject += 1;
if(sizeOfCurrentObject > 2)
goStraight = false;//if we see a large enough object, make the current gap zero
}
}
if(goStraight == true)
return degreesToSweep/2;
//Go over entire array to find the widest opening
for(int j = 0; j <= degreesToSweep; j += 1) {
//if current positions is empty
if(obstacleArray[j] == 0) {
sizeOfCurrentObject = 0;
if(sizeOfCurrentGap == 0)//if we are seeing the beginning of a gap
startOfCurrentGap = j;//set the beginning of hte gap to the current position
//increase the length of the current gap
sizeOfCurrentGap += 1;
} else if(obstacleArray[j] == 1) {
sizeOfCurrentObject += 1;
if(sizeOfCurrentObject > 4)
sizeOfCurrentGap = 0;//if we see a large enough object, make the current gap zero
}
//if the current gap is wider than the longest recorded gap, make it the new longest
if(sizeOfCurrentGap > neededGap) {
//reset the values for the longest
widthOfWidestGap = sizeOfCurrentGap;
startOfWidestGap = startOfCurrentGap;
}
}//end of for loop finding the widest gap
//check to see if the gap is wide enough
if(widthOfWidestGap >= neededGap){
return startOfWidestGap + (widthOfWidestGap / 2);//go to the middle of the widest gap
} else {
if (startOfWidestGap == 0)//if the gap is on the left edge
return startOfWidestGap;//go as far to the left as possible
else if ((startOfWidestGap + widthOfWidestGap) == degreesToSweep)//if the gap is on the right edge
return degreesToSweep;//go as far to the right as possible
//return -1 to say we can't go anywhere
return -1;
}
}
//***************************************************************************
//A method that turns the robot to the left for a given velocity and duration
void turnLeft(uint8_t velocity, int duration) {
uint8_t v = 0;
//Left front and rear wheels reverse spin (Middle wheel acts as pivot)
digitalWrite(dirPins[0], HIGH);
digitalWrite(dirPins[1], HIGH);
digitalWrite(dirPins[2], HIGH);
//All right wheels forward spin
digitalWrite(dirPins[3], LOW);
digitalWrite(dirPins[4], LOW);
digitalWrite(dirPins[5], HIGH);
//Linear Acceleration up to top speed
for(uint8_t i = 0; i < velocity; i++) {
//increase speed by one
v++;
for(byte j = 0; j < 6; j++) {
//update velocities
analogWrite(pwmPins[j], v);
}
delay(5);
}
delay(duration);
brakes(velocity);
}
//***************************************************************************
//A method that turns the robot to the right for a given velocity and duration
void turnRight(uint8_t velocity, int duration) {
uint8_t v = 0;
//All left wheels forward spin
digitalWrite(dirPins[0], LOW);
digitalWrite(dirPins[1], LOW);
digitalWrite(dirPins[2], LOW);
//Right front and rear wheels reverse spin (Middle wheel acts as pivot)
digitalWrite(dirPins[3], HIGH);
digitalWrite(dirPins[4], HIGH);
digitalWrite(dirPins[5], LOW);
//Linear Acceleration up to top speed
for(uint8_t i = 0; i < velocity; i++) {
//increase speed by ten
v++;
for(byte j = 0; j < 6; j++) {
//update velocities
analogWrite(pwmPins[j], v);
}
delay(5);
}
delay(duration);
brakes(velocity);
}
//***************************************************************************
//A method that drives the robot forward for a given velocity and duration
void forward(uint8_t velocity, int duration) {
uint8_t v = 0;
//All six wheels forward spin
digitalWrite(dirPins[0], LOW);
digitalWrite(dirPins[1], LOW);
digitalWrite(dirPins[2], LOW);
digitalWrite(dirPins[3], LOW);
digitalWrite(dirPins[4], LOW);
digitalWrite(dirPins[5], HIGH);
//Linear Acceleration up to top speed
for(uint8_t i = 0; i < velocity; i++) {
//increase speed by ten
v++;
for(byte j = 0; j < 6; j++) {
//update velocities
analogWrite(pwmPins[j], v);
}
delay(5);
}
analogWrite(pwmPins[3], v + 10);
analogWrite(pwmPins[4], v + 10);
analogWrite(pwmPins[5], v + 10);
delay(duration);
brakes(velocity);
}
//***************************************************************************
//A method that drives the robot in reverse for a given velocity and duration
void reverse(uint8_t velocity, int duration) {
uint8_t v = 0;
//All six wheels reverse spin
digitalWrite(dirPins[0], HIGH);
digitalWrite(dirPins[1], HIGH);
digitalWrite(dirPins[2], HIGH);
digitalWrite(dirPins[3], HIGH);
digitalWrite(dirPins[4], HIGH);
digitalWrite(dirPins[5], LOW);
//Linear Acceleration up to top speed
for(uint8_t i = 0; i < velocity; i++) {
//increase speed by ten
v++;
for(byte j = 0; j < 6; j++) {
//update velocities
analogWrite(pwmPins[j], v);
}
delay(5);
}
delay(duration);
brakes(velocity);
}
void brakes(uint8_t velocity) {
int v = velocity;
for(uint8_t i = 0; i < velocity; i++) {
v--;
for(byte j = 0; j < 6; j++) {
analogWrite(pwmPins[j], v);
}
delay(5);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment