The process for mounting my sensor started with cutting another piece of wood. I also had to shave down one of my spacers because it was too thick. From there it was simply a matter of pushing screws through the sensor's screw holes, and using nuts to connect it with the newly cut wood. Using a right-angle bracket, I screwed the wood into the piece atop my servo. And voila, it works!
If only. To get the result of the video above requires programming. This programming I'd have loved to have done a month ago, but my car was not exactly finished back then. So I've been writing a lot of code in a couple days. The code for the Arduino is a form of C++ that is modified to be easier to code in. I have done a significant amount of coding already. If not for what is hopefully just a small issue, I could have also uploaded a video of my car not hitting a wall.That issue is one of a faulty sensor. However, the sensor isn't always faulty - it only appears to have issues when I'm also driving the car's motors. The reason for this is one of voltage spikes (I hope) - the sensor's readings get messed up when the motors start because of the change in the distribution of power. I do have a way to fix it, though. The picture below is from the sensor's manufacturer showing exactly how to solve this sort of issue. And hopefully that is the only sort of issue, as the sensor normally works fine.
/* Senior Project
* Autonomous Car
* Code by Connor Woodson
* Started 05/18/11
*/
#include <Servo.h>
/** CONSTANTS **/
const int READING_NUM = 3; // Number of readings to be taken to insure accuracy
const int SERVO_STEP = 10; // Degrees the servo moves for every reading
/** INPUT PORTS **/
const int btnPort = 13;
const int sonarPort = 2;
/** OUTPUT PORTS **/
const int servoPort = 3;
const int PWMA_PIN = 11; // Motor A = Drive Motor
const int AIN1_PIN = 10;
const int AIN2_PIN = 9;
const int STBY_PIN = 8;
const int PWMB_PIN = 5; // Motor B = Steer Motor
const int BIN1_PIN = 6;
const int BIN2_PIN = 7;
const int lightOne = 12;
const int lightTwo = 4;
/** GLOBAL VARIABLES **/
Servo scanner;
boolean lastBtnState = false; // Holds state of the start/stop button
boolean btnState = false;
int currentState = 0;
int currentDirection = 0;
int servoAngle = 0;
int servoDirection = 1;
int distanceReadings[(180 / SERVO_STEP) + 1]; // Fencepost Rule
void setup()
{
Serial.begin(9600);
}
void loop()
{
lastBtnState = btnState;
btnState = digitalRead(btnPort) == HIGH;
boolean btnReleased = (!btnState && lastBtnState);
switch(currentState)
{
case 0: // Waiting to be told to start
if(btnReleased)
currentState = 1;
break;
case 1: // Prepare car to go
startCar();
currentState = 2;
break;
case 2: // Running AI
if(btnReleased || runCar())
currentState = 3;
break;
case 3: // Told to stop
driveMotorStop();
steerStraight();
scanner.write(90);
delay(500);
scanner.detach();
motorStandby(true);
digitalWrite(lightOne, LOW);
digitalWrite(lightTwo, LOW);
currentState = 0;
break;
}
}
void startCar()
{
if(scanner.attached())
scanner.detach();
scanner.attach(servoPort, 500, 2200); // Servo range is 500 -> 2200 microseconds
// ready pins
pinMode(btnPort, INPUT);
pinMode(sonarPort, INPUT);
pinMode(PWMA_PIN, OUTPUT);
pinMode(AIN1_PIN, OUTPUT);
pinMode(AIN2_PIN, OUTPUT);
pinMode(PWMB_PIN, OUTPUT);
pinMode(BIN1_PIN, OUTPUT);
pinMode(BIN2_PIN, OUTPUT);
pinMode(STBY_PIN, OUTPUT);
pinMode(lightOne, OUTPUT);
pinMode(lightTwo, OUTPUT);
//motorStandby(false);
//driveMotorGo(40);
steerStraight();
fullScan();
setServo(90, 1);
delay(500);
}
boolean runCar()
{
moveServo();
return false;
}
void motorStandby(boolean yes)
{
if(yes)
digitalWrite(STBY_PIN, LOW);
else
digitalWrite(STBY_PIN, HIGH);
}
/** DRIVING COMMANDS **/
void driveMotorGo(int vel)
{
byte PWMvalue = map(abs(vel), 0, 100, 0, 255);
if(vel == 0)
{
driveMotorStop();
return;
}
else if(vel > 0)
{
digitalWrite(AIN1_PIN, HIGH);
digitalWrite(AIN2_PIN, LOW);
}
else if(vel < 0)
{
digitalWrite(AIN1_PIN, LOW);
digitalWrite(AIN2_PIN, HIGH);
}
analogWrite(PWMA_PIN, PWMvalue);
}
void driveMotorStop()
{
digitalWrite(AIN1_PIN, LOW);
digitalWrite(AIN2_PIN, LOW);
}
/** STEERING COMMANDS **/
void steerLeft()
{
if(currentDirection == 1)
return;
currentDirection = 1;
digitalWrite(BIN1_PIN, HIGH);
digitalWrite(BIN2_PIN, LOW);
analogWrite(PWMB_PIN, 255);
}
void steerRight()
{
if(currentDirection == -1)
return;
currentDirection = -1;
digitalWrite(BIN1_PIN, LOW);
digitalWrite(BIN2_PIN, HIGH);
analogWrite(PWMB_PIN, 255);
}
void steerStraight()
{
if(currentDirection == 0)
return;
currentDirection = 0;
digitalWrite(BIN1_PIN, LOW);
digitalWrite(BIN2_PIN, LOW);
}
/** SONAR SENSOR FUNCTIONS **/
int getDistance()
{
int dist = modeFilter(sonarSortedList(
if(dist < 12) // Display distance using two lights in binary (close, less close, far, very far)
{
digitalWrite(lightOne, HIGH);
digitalWrite(lightTwo, HIGH);
}
else if(dist < 36)
{
digitalWrite(lightOne, LOW);
digitalWrite(lightTwo, HIGH);
}
else if(dist < 100)
{
digitalWrite(lightOne, HIGH);
digitalWrite(lightTwo, LOW);
}
else
{
digitalWrite(lightOne, LOW);
digitalWrite(lightTwo, LOW);
}
return dist;
}
int readDistance()
{
return pulseIn(sonarPort, HIGH) / 147;
//return analogRead(sonarPort) / 2;
}
// Grabs given number of readings, and sorts them by an insertion sort.
int* sonarSortedList(int listSize)
{
if(listSize <= 0)
{
int dists[] = {0};
return dists;
}
int dists[listSize];
dists[0] = readDistance();
for(int i = 1; i < listSize; i++)
{
dists[i] = readDistance();
int a = i;
while(dists[a] > dists[a-1])
{
// Swap
int temp = dists[a];
dists[a] = dists[a-1];
dists[a-1] = temp;
a--;
if(a == 0)
break;
}
}
return dists;
}
// Returns middle of a sorted list of readings
int medianFilter(int values[], int arrSize)
{
if(arrSize <= 0)
return 0;
return (values[arrSize / 2] + values[(arrSize + 1) / 2]) / 2;
}
// Returns most frequently occurring distance from sorted list of readings
int modeFilter(int values[], int arrSize)
{
if(arrSize <= 0)
return 0;
int streak = 1;
int maxStreak = 1;
int maxStreakNum = values[0];
for(int i = 1; i < arrSize; i++)
{
if(values[i] == values[i - 1])
streak++;
else
{
if(streak > maxStreak)
{
maxStreak = streak;
maxStreakNum = values[i - 1];
}
streak = 1;
}
}
if(maxStreak == 1)
return medianFilter(values, arrSize); // No recurring distance, so use median
else
return maxStreakNum;
}
/** SERVO COMMANDS **/
void runLogic()
{
moveServo();
int arrayElmt = servoAngle / SERVO_STEP;
distanceReadings[arrayElmt] = getDistance();
}
// Steps the servo one amount, and takes a reading
void moveServo()
{
if(!scanner.attached())
{
Serial.println("Moving Servo: Servo Detached");
return;
}
Serial.print("Moving servo from: ");
Serial.print(servoAngle);
if(((servoAngle + (servoDirection * SERVO_STEP)) > 180) || ((servoAngle + (servoDirection * SERVO_STEP)) < 0)) // Swap direction
servoDirection *= -1;
servoAngle += (servoDirection * SERVO_STEP);
scanner.write(servoAngle);
Serial.print(" to: ");
Serial.print(servoAngle);
Serial.print("; Direction: ");
Serial.println(servoDirection)
delay(10 * SERVO_STEP); // Replace with timed calls
}
// Takes a full scan of the surroundings
void fullScan()
{
Serial.println("Starting full scan");
if(!scanner.attached())
{
Serial.println("Servo Detached");
return;
}
setServo(0, 1);
delay(100);
for(int i = 0; i < (180 / SERVO_STEP); i++)
moveServo();
Serial.println("Finished full scan.");
}
void setServo(int angle, int dir)
{
if(!scanner.attached())
{
Serial.println("Setting Servo: Servo Detached");
return;
}
if(dir < 0)
servoDirection = -1;
else
servoDirection = 1;
if(angle > 180)
angle = 180;
else if(angle < 0)
angle = 0;
scanner.write(angle);
servoAngle = angle;
Serial.print("Setting servo to: ");
Serial.print(servoAngle);
Serial.print("; Direction: ");
Serial.println(servoDirection)
}
So the start of the code is just a whole bunch of variables and constants. There are a couple constants to allow fine-tuning of the car, but most of them are the port numbers on the Arduino into which various wires are attached. Each motor requires three wires passed to the motor driver: IN1 and IN2 are set either HIGH/LOW or LOW/HIGH to indicate which direction the motor should turn (they are digital ports, meaning they are binary and thus only have two states; HIGH is an output of, say, 5V and LOW is an output of 0V). PWM is pulse-width modulation, which indicates the speed of the motor. PWM is where the output is toggled On/Off over a set of time. The higher the PWM output, the more of that time is spent being HIGH then LOW (the Arduino site has a good explanation of this).
There are two required methods in any Arduino program (or sketch, as it is called): setup and loop. Setup is called when the program starts. In my setup method, I'm merely connecting the Arduino to my computer through a serial port to allow for debug outputs. All of my normal setup routines occur in the loop function. The reason for this is I've implemented a button that allows me to tell the car to start and stop, without having to turn the power off every time. My loop function behaves differently depending on which state it is in (held in a simple integer variable), allowing me to have start and end routines for my car without leaving the program.
Moving on to my motor control. My motor driver has a standby mode which, if set, turns power off to the motors. By default this mode is turned on, meaning that I have to manually turn it off for my motors to work. The drive motor is controlled by giving a function a velocity from +/- 0-100, which the function then maps to a PWM value of 0-255 (as that's a byte of data). There is also a function to brake the drive motor. For steering, I have three functions: steerLeft, steerRight, steerStraight. As I had discovered previously, for my steering motor to work, I just turn it on full power (a PWM signal of 255 meaning always HIGH). These functions just set the direction the motor turns (steerStraight turns the motor off).
My code is a little more complex when it comes to my sonar sensor. To start, I have a function getDistance which returns the current distance detected by the sensor. Right now it is a little more complicated, because I have two LEDs which I'm trying to use to communicate the latest reading. The function readDistance interfaces directly with the sensor. The point of this function is in case I have to change that interface (right now it is PWM, but it can also be analog voltage or serial communication), I only have to change one line. The function that calls readDistance is an insertion sort, meaning that, for a given number of entries, reads from the scanner and places them in ascending order into an array. I have two filters as well to control the quality of my sensor's data. The medianFilter takes the array of values and returns the median value. The modeFilter returns the most commonly occurring distance, or if no distance occurs more than once, returns the medianFilter.
For my servo, the function moveServo will be the most commonly called. This takes the constant SERVO_STEP and changes the angle of the servo by that amount. It will also reverse the direction of the servo if necessary (meaning, if the servo is moving to the left but is at 0 degrees, it will move to the right for the next turn). I have the function fullScan which takes the servo and scans a full 180 degrees (following SERVO_STEP's value). This function will require a little more work, as the purpose of it is to initially populate an array with everything in front of the car.
So with my three major components down - motors, sensor, servo - I just need to come up with some functions that tie all of those together. Hopefully that won't take too long, and I'll have a working car quite soon!
'Til then.
Next: Journal Entry #8.5
Sources: [1]
