I have built a robot to follow a black line (the track) using IR sensors. Throughout the track there are various locations where the robot will have to pick something up (with a gripper that is attached to an arm that is attached to a servo), drop the item off somewhere else, and turn around to go get another item and repeat the above process. I locations of the items and drop-off locations have black tape so an IR sensor will be able to detect and based on the iteration of 'i' in my code, will determine which task to perform. The problem is that my robot just starts the stops then starts then stops and just does some really funky stuff. The readings from the Serial Monitor are not ideal at all. Can someone review the code and give some advice? Maybe some delays are necessary in certain spots, maybe some need removed, or maybe my logic just doesn't make sense. Any advice is appreciated.
Void Loop is what I'm most concerned about but if there are any obvious programming errors, please let me know too. It does compile.
IR Sensor Layout:
1---------2--3--4---------5
Direction of travel can be thought of as going upwards on the screen. ^^^
1 --> Senses tape to perform iterations
2,3,4 --> Control basic traveling stabilization on the track
5 --> Senses tape after robot has turned around to perform iterations. Also detects location for robot to turn around.
#include <Servo.h>
Servo servo; Servo gripper; int i = 1;
void turnaround(); void grabitem(); void releaseitem(); // Defines functions
int pasn = 6; int pasp = 9; int drip = 10; int drin = 11; // Outputs
int ir1 = 2; int ir2 = 4; int ir3 = 7; int ir4 = 8; int ir5 = 12; // Inputs
// ir1: Driver outside, ir2: Driver outside-center, ir3: Center, ir4: Passenger outside-center, ir5: Passenger outside
int passpeed = 3; int drispeed = 5; // Outputs
void setup(){
servo.attach(A0); // Servo
gripper.attach(A1); // Gripper
pinMode(pasn, OUTPUT); pinMode(pasp, OUTPUT); pinMode(drip, OUTPUT); pinMode(drin, OUTPUT);
Serial.begin(9600);
pinMode(ir1, INPUT); pinMode(ir2, INPUT); pinMode(ir3, INPUT); pinMode(ir4, INPUT); pinMode(ir5, INPUT);
}
void loop(){
delay(250);
drive();
Serial.print(i);
if(digitalRead(ir1)){ // IR 1 senses black tape
if(i==1 || i==6){ // Pick up item at iteration 1 OR 6
analogWrite(passpeed, 0);
analogWrite(drispeed, 0);
delay(500);
grabitem();
drive();
}else if(i==2 || i==4 || i==5 || i==7){ // Keep moving
drive();
}else if(i==3 || i==8){ // Drop off item at iteration 3 OR 8
analogWrite(passpeed, 0);
analogWrite(drispeed, 0);
delay(500);
if(i==3){
releaseitem();
turnaround();
drive();
}else if(i==8){
releaseitem();
drive();
}
}
}else if(i==9){ // Robot stops and senses to see if it should knock tube over
delay(500);
analogWrite(passpeed, 0);
analogWrite(drispeed, 0);
// Detect cylinder height
drive();
}else if(i==10){ // Robot stops and senses to see if it should knock tube over
analogWrite(passpeed, 0);
analogWrite(drispeed, 0);
// Detect cylinder height
drive();
}else if(i==11){ // Robot stops and senses to see if it should knock tube over
analogWrite(passpeed, 0);
analogWrite(drispeed, 0);
// Detect cylinder height
drive();
}else if(i==12){ // Robot stops and senses to see if it should knock tube over
analogWrite(passpeed, 0);
analogWrite(drispeed, 0);
// Detect cylinder height
drive();
}
delay(500);
I++
}
void drive(){
Serial.println("Driving");
if(digitalRead(ir3) && !digitalRead(ir1) && !digitalRead(ir5)){
// Robot goes straight
analogWrite(passpeed, 200);
analogWrite(drispeed, 200);
digitalWrite(drip, HIGH);
digitalWrite(drin, LOW);
digitalWrite(pasp, HIGH);
digitalWrite(pasn, LOW);
}
else if(digitalRead(ir2) && !digitalRead(ir3) && !digitalRead(ir4)&& !digitalRead(ir1)){
// Goes left
analogWrite(passpeed, 250);
analogWrite(drispeed, 250);
digitalWrite(drip, LOW);
digitalWrite(drin, HIGH);
digitalWrite(pasp, HIGH);
digitalWrite(pasn, LOW);
}
else if(digitalRead(ir3) && digitalRead(ir2) && !digitalRead(ir4)&& !digitalRead(ir1)){
// Goes left
analogWrite(passpeed, 200);
analogWrite(drispeed, 250);
digitalWrite(drip, LOW);
digitalWrite(drin, HIGH);
digitalWrite(pasp, HIGH);
digitalWrite(pasn, LOW);
}
else if(digitalRead(ir3) && digitalRead(ir4) && !digitalRead(ir2) && !digitalRead(ir1)){
// Goes right
analogWrite(passpeed, 200);
analogWrite(drispeed, 250);
digitalWrite(drip, HIGH);
digitalWrite(drin, LOW);
digitalWrite(pasp, LOW);
digitalWrite(pasn, HIGH);
}
else if(digitalRead(ir4) && !digitalRead(ir3) && !digitalRead(ir2)&& !digitalRead(ir1)){
// Goes right
analogWrite(passpeed, 200);
analogWrite(drispeed, 250);
digitalWrite(drip, HIGH);
digitalWrite(drin, LOW);
digitalWrite(pasp, LOW);
digitalWrite(pasn, HIGH);
}
}
void turnaround(){
Serial.println("Robot Turning Around");
analogWrite(passpeed, 100);
analogWrite(drispeed, 100);
digitalWrite(pasp, HIGH);
digitalWrite(drip, LOW);
digitalWrite(drin, HIGH);
digitalWrite(pasn, LOW);
delay(500);
if(digitalRead(ir3)){
drive();
}
}
void grabitem(){
Serial.println("Gripping Item");
delay(500);
servo.write(1); // Robot arm prepares to grab item
delay(250);
gripper.write(180); // Gripper Closees
delay(250);
servo.write(90); // Robot arm returns to home position
delay(500);
}
void releaseitem(){
Serial.println("Releasing Item");
delay(500);
servo.write(1); // Robot arm prepares to drop item
delay(250);
gripper.write(60); // Gripper opens
delay(250);
servo.write(90); // Robot arm returns to home position
delay(500);
}
