3.

0 Microcontroller Program Design

3.1 Flowchart
Left Wall Algorithm

3.2 Code
/*
KilaBOT Final Program
Members:
Alvin
Virj
Brendy
Pomoy
Ken
Goldwyn
Tapec
*/
#include
#include
#include
#include
#include

<NewPing.h>
<AccelStepper.h>
<Wire.h>
<Adafruit_MotorShield.h>
"utility/Adafruit_PWMServoDriver.h"

// sets the Sensor's designated Digital pins (Trigger, Echo, Max. Distance)
NewPing PingFront(7, 6, 400);
NewPing PingLeft(5, 4, 400);
NewPing PingRight(3, 2, 400);
// calls the Motorshield to be recognized by the Arduino
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_StepperMotor *Rmotor = AFMS.getStepper(200, 2);
Adafruit_StepperMotor *Lmotor = AFMS.getStepper(200, 1);

// customized functions to simplify the flow of the program

void forward(){
Rmotor->onestep(FORWARD, DOUBLE);
Lmotor->onestep(BACKWARD, DOUBLE);
}
void left(){
Rmotor->onestep(FORWARD, DOUBLE);
Lmotor->onestep(FORWARD, DOUBLE);
}
void right(){
Rmotor->onestep(BACKWARD, DOUBLE);
Lmotor->onestep(BACKWARD, DOUBLE);

}
void setup(){
Serial.begin(115200);
AFMS.begin();
Rmotor->setSpeed(100); // Sets the max speed for Right motor
Lmotor->setSpeed(100); // Sets the max speed for Left motor
delay(2000);
}
void loop(){
// Check if there is a wall on the Left
unsigned int uS2 = PingLeft.ping();
unsigned int PINGLEFT = uS2 / US_ROUNDTRIP_CM;
// If there is a wall on the Left
if(PINGLEFT<15){
// Check if there is a wall on the Front
unsigned int uS1 = PingFront.ping();
unsigned int PINGFRONT = uS1 / US_ROUNDTRIP_CM;
// If there is a wall on the Front
if(PINGFRONT<7){
// Check if there is a wall on the Right
unsigned int uS3 = PingRight.ping();
unsigned int PINGRIGHT = uS3 / US_ROUNDTRIP_CM;
// If there is a wall on the Right
if(PINGRIGHT<15){
// 360deg Left Turn
for(int i=0; i<161; i++){
left();
}
}
// If there is no wall on the Right
else if(PINGRIGHT>=15){
// 90deg Right Turn
for(int i=0; i<81; i++){
right();
}
}
}

// If there is no wall on the Front

else if(PINGFRONT>=7){
// Moves one step forward
forward();
}
}
// If there is no wall on the Left
else if(PINGLEFT>=15){
// Moves Forward so that it will stay at the middle
for(int i=0; i<30; i++){
forward();
}
// 90deg Left Turn
for(int i=0; i<81; i++){
left();
}
// Moves Forward again so the Robot will enter a new square
for(int i=0; i<180; i++){
forward();
}
}
}