We know how to control our 2 motors in both directions and we also know how to use our Phototransistors to detect if an object is close, now we put both of these together to form our robot. In this part we focus more on producing the code as we’ve already put our robot together as below.
An easy way to produce some code is to break things up into simple terms like what we have below.
Turn both motors on to move forward
If left sensor detects wall, turn left motor on forward and right motor on reverse
If right sensor detects wall, turn right motor on forward and left motor on reverse
Simple, isn’t it? Now this would only move left or right as long as the wall keeps being detected, this can be good and bad at the same time. The robot could detect the wall and move away only a little so it stays perpendicular to the wall, the other way we could do it is add a timer when moving left and right so that it continues to move even though there is no wall detected.
Now for the code breakdown, first we have our standard variables and the setup method. As you see we have 2 pins assigned to the sensors, 4 pins for the 2 directions of each motor and also have 2 variables to keep the direction that each motor is spinning in.
const int sensorLeft = 0;
const int sensorRight = 1;
const int motorLeftForward = 2;
const int motorRightForward = 3;
const int motorLeftReverse = 4;
const int motorRightReverse = 5;
const int ledPin = 13;
int sensorLeftValue = 0;
int sensorRightValue = 0;
int motorLeftDirection = 1;
int motorRightDirection = 1;
void setup() {
//Serial.begin(9600);
// Initialise the digital pins as an output
pinMode(ledPin, OUTPUT);
pinMode(motorLeftForward, OUTPUT);
pinMode(motorRightForward, OUTPUT);
pinMode(motorLeftReverse, OUTPUT);
pinMode(motorRightReverse, OUTPUT);
}
Then we have our loop as standard, we firstly read the analog values of both sensors. You can uncomment the serial.print lines to have them print the values to the serial monitor.
void loop() {
// Read the analog in from the sensors
sensorLeftValue = analogRead(sensorLeft);
sensorRightValue = analogRead(sensorRight);
// Print the results to the serial monitor
//Serial.print("sensorLeft = " );
//Serial.print(sensorLeftValue);
//Serial.print("sensorRight = " );
//Serial.print(sensorRightValue);
//Serial.print("\n");
Now we set both motors to move forward so the robot always moves forward if it doesn’t detect any walls.
motorLeftDirection = 1; motorRightDirection = 1;
Here is where we do the sensing, you might have to adjust the values to suit. For me, when my phototransistors detect something they drop down from 1000, so that’s my detection point. So if the left sensor detects a near by wall, we’ll firstly make the led on the Arduino light up (you will barely see if it detects the walls) then we say we want the motor on the left to go forward and the motor on the right to spin backwards. If we detect a wall on the right, do the opposite. It’s important to note the else if, because we don’t want our robot detecting walls on both sides and then either doing nothing or going forward.
if (sensorLeftValue < 1000) {
digitalWrite(ledPin, HIGH);
motorLeftDirection = 1;
motorRightDirection = 0;
}
else if (sensorRightValue < 1000) {
digitalWrite(ledPin, HIGH);
motorLeftDirection = 0;
motorRightDirection = 1;
}
Now is when we activate the motors according on the direction specified by motorLeftDirection and motorRightDirection. If it’s 1, we run that motor direction as forward, if it’s 0 we run the motor direction as reverse. We must make sure that we set a LOW to the opposite direction before spinning the motor in the right direction, otherwise the set values in previous loops might cause issues (e.g. we set motorLeftReverse to HIGH in a previously loop, we didn’t clear it in our current loop and then set motorLeftForward to HIGH, this would make the motor spin in no direction at all. It may also cause a short circuit).
// Activate left motor
if (motorLeftDirection == 1) {
digitalWrite(motorLeftReverse, LOW);
digitalWrite(motorLeftForward, HIGH);
}
else {
digitalWrite(motorLeftForward, LOW);
digitalWrite(motorLeftReverse, HIGH);
}
// Activate right motor
if (motorRightDirection == 1) {
digitalWrite(motorRightReverse, LOW);
digitalWrite(motorRightForward, HIGH);
}
else {
digitalWrite(motorRightForward, LOW);
digitalWrite(motorRightReverse, HIGH);
}
Now we just delay the code just enough so that it can still sense walls at a timely rate and can deliver current to the motors.
delay(10); digitalWrite(ledPin, LOW); }
This entire loop above will run 100 times a second: 1,000ms (1 second) / 10ms delay = 100 times. Below is a video of the robot in action.
[youtube=https://www.youtube.com/watch?v=pJLYBxE1s1c&hl=en_US&fs=1&]As you can see it works pretty well except that you see it detects the walls, then moves a little, detects walls and then moves a little more which means it can sometimes be perpendicular to a wall and not detect it as we would like. We can add a timer delay to make the movement happen more easily and make it turn left or right a minimum amount of times if it detects any walls by modifying the code slightly as below.
int motorLeftDirection = 1; int motorRightDirection = 1; int motorTimer = 0;
if (motorTimer == 0) {
motorLeftDirection = 1;
motorRightDirection = 1;
digitalWrite(ledPin, LOW);
}
if (motorTimer > 0) {
motorTimer--;
}
if (sensorLeftValue < 1000) {
digitalWrite(ledPin, HIGH);
motorLeftDirection = 1;
motorRightDirection = 0;
motorTimer = 20;
}
else if (sensorRightValue < 1000) {
digitalWrite(ledPin, HIGH);
motorLeftDirection = 0;
motorRightDirection = 1;
motorTimer = 20;
}
Now we see in the below video it works a bit better. You can increase value of motorTimer to say 40 or so to see a better improvement.
We can continue to make improvements like add a knock sensor or phototransistor on the front of the robot because at the moment the 2 phototransistors can’t detect a wall straight in front as it’s too far away. Another thing we could do is replace the third direction wheel at the front with a servo so we can precisely direct the robot in the way we want. This would help to make the robot move in a straight line after it detects a wall and turns.
If we didn’t want to use a servo or wanted a cheaper alternative, we could add a little strip of white paper or any other material to the top of the middle wheel and then use a phototransistor to detect the white paper when the robot is moving straight (e.g. if the phototransistor was at 500 it is straight, if at 800 the robot is turning left, if at 300 the robot is turning right). Look at the blue tack on the middle wheel that’s where you would mount the paper.
So that’s all for our Robot, it’s at a working state. I said I was going to build my own motor controller and I’ve now got all the parts I need so I’ll be posting about that next which will help anyone out who doesn’t have a Cycbot motor controller like I do, then after that I will most likely make a PCB of the whole circuit (I’m also thinking about producing cheap motor controllers and on selling them but that’s down the track) 🙂
