#include <Stepper.h> //Include library for the Stepper motor. This is the default Library that comes with Arduino.
const int stepsPerRevolution = 2048; //Number of steps per revolution (2048 --- 64:1 Gear ratio, 32 initial steps per revolution... 32*64==2048)
const int ENB = 5; //Enable side B
const int ENA = 6; //Enable side A
const int IN1 = 11; //Init #1
const int IN2 = 9; //Init #2
const int IN3 = 8; //Init #3
const int IN4 = 7; //Init #4
//IN1-IN4 I reversed the pin numbers, because that is how they are shown on the board.
//Variable Integers:
int brushSpeed; //The speed of the brush
int swLeft; //These switches are ordered from left to right when the bot is facing away from you.
int swRight;
Stepper brush(stepsPerRevolution, 4, 12, 10, 13);
void lTurn(int speed, int length) { //Turn left
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, speed);
analogWrite(ENB, speed);
delay(length);
}
void rTurn(int speed, int length) { //Turn right
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENA, speed);
analogWrite(ENB, speed);
delay(length);
}
void fDrive(int speed, int length) { //Drive forwards
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
analogWrite(ENA, speed);
analogWrite(ENB, speed);
delay(length);
}
void bDrive(int speed, int length) { //Drive backwards
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
analogWrite(ENA, speed);
analogWrite(ENB, speed);
delay(length);
}
void brake(int length) { //Stop moving
digitalWrite(ENA, LOW);
digitalWrite(ENB, LOW);
delay(length);
}
//New Functions:
void blockageLeft(int numTrigger) { //In the event of an object blocking the robot's path on the left side:
if (numTrigger == 1) {
Serial.println("Began Left Blockage");
brushSpeed=0;
brake(20);
bDrive(255, 1500);
rTurn(255, 650);
Serial.println("Ended Left Blockage");
}
}
void blockageRight(int numTrigger) { //In the event of an object blocking the robot's path on the right side
if (numTrigger == 1) {
Serial.println("Began Right Blockage");
brushSpeed=0;
brake(20);
bDrive(255, 1500);
lTurn(255, 650);
Serial.println("Ended Right Blockage");
}
}
void setup() {
pinMode(IN1, OUTPUT); //To declare each pin an input or output
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(0, INPUT_PULLUP);
pinMode(3, INPUT_PULLUP);
Serial.begin(9600);
}
void loop() {
Serial.println("Began Loop");
brushSpeed=15;
fDrive(215, 75);
swLeft = digitalRead(0);
swRight = digitalRead(3);
if (swLeft == LOW) {
blockageLeft(1);
}
if (swRight == LOW) {
blockageRight(1);
}
Serial.println("Test Point 1");
brush.setSpeed(brushSpeed); //RPMs equal brushSpeed integer
Serial.println("Test Point 2");
brush.step(-stepsPerRevolution);
Serial.println("Ended Loop");
}
Robot_Vacuum_Offline.ino (3.2 KB)