State Diagram
- Below is a picture of the state diagram we implemented in software. It closely follows the path of our solution strategy, as discussed in the Designs section. Events associated with state transitions are in unbolded rectangles, and states are in bolded rectangles.
Brain Code
This code went to the Arduino that controlled the logic, and read from the ultrasonic sensors
/*************************************************************
File: brain_arduino
Contents: Manages state of the robot by communicating with motor arduino,
collecting sensor info and controlling the shooting electronics.
Notes: Target: Arduino Uno
Arduino IDE version: 1.6.7
*************************************************************/
/*---------------Includes-----------------------------------*/
#include <TimerOne.h>
#include <NewPing.h>
#include <Wire.h>
#include <Servo.h>
/*---------------Function Prototypes-----------------------------------*/
void escape_safe_space();
void send_motor_command(byte code);
void move_to_distance(int thresh, NewPing sonar, bool moving_towards);
void timer_interrupt(void);
void shoot(void);
void nav_spot_one(void);
void nav_spot_two(void);
void fact_check(void);
void return_safe_space(void);
/*---------------Module Defines-----------------------------*/
#define MAX_PLAY_TIME 130
#define MAX_DISTANCE 250 // Maximum distance we want to ping for (in centimeters).
#define BACK_WALL_THRESH 12 //cm
#define SPOT_ONE_THRESH 61-15-1 //cm
#define SPOT_TWO_THRESH 122-15 //cm
#define FACT_CHECK_THRESH 10 //cm
#define CENTER_LINE_THRESH 61 //cm
#define WALL_CLOSE_THRESH 12 //cm
#define WALL_PAUSE_MS 2000
#define init_pos 10
#define rotation 28
/*---------------Sensor Pin Definitions-----------------------------*/
#define SERVO_PIN 2
#define SHOOT_EN 3
#define TRIGGER_PIN_F 6
#define ECHO_PIN_F 7
#define TRIGGER_PIN_B 8
#define ECHO_PIN_B 9
#define TRIGGER_PIN_L 10
#define ECHO_PIN_L 11
#define TRIGGER_PIN_R 12
#define ECHO_PIN_R 13
/*---------------Globals-----------------------------*/
Servo myservo;
volatile unsigned long seconds_passed = 0;
int pos; //servo position
int shooter_speed = 95;
bool fact_checked = false;
NewPing sonar_F(TRIGGER_PIN_F, ECHO_PIN_F, MAX_DISTANCE);
NewPing sonar_B(TRIGGER_PIN_B, ECHO_PIN_B, MAX_DISTANCE);
NewPing sonar_L(TRIGGER_PIN_L, ECHO_PIN_L, MAX_DISTANCE);
NewPing sonar_R(TRIGGER_PIN_R, ECHO_PIN_R, MAX_DISTANCE);
void setup() {
Serial.begin(9600);
Wire.begin();
//Timer1.initialize(); //1 sec period
//Timer1.attachInterrupt(timer_interrupt);
myservo.attach(SERVO_PIN);
myservo.attach(SERVO_PIN);
pos = init_pos;
myservo.write(init_pos);
//send_motor_command('s');
}
void loop() {
escape_safe_space();
nav_spot_one();
nav_spot_two();
fact_check();
return_safe_space();
}
void reload(void){
send_motor_command('r');
move_to_distance(CENTER_LINE_THRESH-15, sonar_L, false);
send_motor_command('b');
move_to_distance(BACK_WALL_THRESH - 5, sonar_B, true);
delay(7500);
}
void return_safe_space(void){
send_motor_command('l');
move_to_distance(CENTER_LINE_THRESH +15, sonar_L, true);
send_motor_command('b');
move_to_distance(BACK_WALL_THRESH - 5, sonar_B, true);
delay(3000);
}
void fact_check(void){
send_motor_command('r');
delay(5000);
fact_checked = true;
}
void nav_spot_two(void){
send_motor_command('f'); //forward
move_to_distance(SPOT_TWO_THRESH, sonar_B, false);
send_motor_command('l');
delay(500);
push_to_wall('x');
shoot();
myservo.write(init_pos);
pos = init_pos;
}
void nav_spot_one(void){
send_motor_command('f'); //forward
move_to_distance(SPOT_ONE_THRESH, sonar_B, false);
send_motor_command('l');
delay(500);
push_to_wall('x');
shoot();
}
void escape_safe_space(void){
int distance = sonar_B.ping_cm();
if(!fact_checked){
send_motor_command('t'); //turn
move_to_distance(BACK_WALL_THRESH, sonar_B, true);
}
send_motor_command('b'); //move back
delay(500);
push_to_wall('y'); //push backwards
delay(500);
send_motor_command('l'); //move left
move_to_distance(WALL_CLOSE_THRESH, sonar_L, true);
send_motor_command('s');
delay(2000);
push_to_wall('x');
}
void send_motor_command(byte code){
Wire.beginTransmission(8); // transmit to device #8
Wire.write(code); // sends one byte
Wire.endTransmission(); // stop transmitting
}
void releaseBall(){
for (int x = pos; x < pos + rotation ; x += 1) {
myservo.write(x);
delay(25);
}
pos += rotation;
}
void shoot(void){
send_motor_command('s');
delay(500);
analogWrite(SHOOT_EN, shooter_speed);
delay(8000);
for(int i = 0; i<3; i++){
releaseBall();
delay(3000);
}
analogWrite(SHOOT_EN , 0);
}
//waits to reach threshold, the returns
void move_to_distance(int thresh, NewPing sonar, bool moving_towards){
int distance = 0;
while(true){
delay(100);
distance = sonar.ping_cm();
//Serial.println(distance);
if(moving_towards){
if((distance != 0) && (distance <= thresh)) {
break;
}
}
else{
if((distance != 0) && (distance >= thresh)) {
break;
}
}
}
}
void push_to_wall(byte code){
send_motor_command(code);
delay(WALL_PAUSE_MS);
}
File: brain_arduino
Contents: Manages state of the robot by communicating with motor arduino,
collecting sensor info and controlling the shooting electronics.
Notes: Target: Arduino Uno
Arduino IDE version: 1.6.7
*************************************************************/
/*---------------Includes-----------------------------------*/
#include <TimerOne.h>
#include <NewPing.h>
#include <Wire.h>
#include <Servo.h>
/*---------------Function Prototypes-----------------------------------*/
void escape_safe_space();
void send_motor_command(byte code);
void move_to_distance(int thresh, NewPing sonar, bool moving_towards);
void timer_interrupt(void);
void shoot(void);
void nav_spot_one(void);
void nav_spot_two(void);
void fact_check(void);
void return_safe_space(void);
/*---------------Module Defines-----------------------------*/
#define MAX_PLAY_TIME 130
#define MAX_DISTANCE 250 // Maximum distance we want to ping for (in centimeters).
#define BACK_WALL_THRESH 12 //cm
#define SPOT_ONE_THRESH 61-15-1 //cm
#define SPOT_TWO_THRESH 122-15 //cm
#define FACT_CHECK_THRESH 10 //cm
#define CENTER_LINE_THRESH 61 //cm
#define WALL_CLOSE_THRESH 12 //cm
#define WALL_PAUSE_MS 2000
#define init_pos 10
#define rotation 28
/*---------------Sensor Pin Definitions-----------------------------*/
#define SERVO_PIN 2
#define SHOOT_EN 3
#define TRIGGER_PIN_F 6
#define ECHO_PIN_F 7
#define TRIGGER_PIN_B 8
#define ECHO_PIN_B 9
#define TRIGGER_PIN_L 10
#define ECHO_PIN_L 11
#define TRIGGER_PIN_R 12
#define ECHO_PIN_R 13
/*---------------Globals-----------------------------*/
Servo myservo;
volatile unsigned long seconds_passed = 0;
int pos; //servo position
int shooter_speed = 95;
bool fact_checked = false;
NewPing sonar_F(TRIGGER_PIN_F, ECHO_PIN_F, MAX_DISTANCE);
NewPing sonar_B(TRIGGER_PIN_B, ECHO_PIN_B, MAX_DISTANCE);
NewPing sonar_L(TRIGGER_PIN_L, ECHO_PIN_L, MAX_DISTANCE);
NewPing sonar_R(TRIGGER_PIN_R, ECHO_PIN_R, MAX_DISTANCE);
void setup() {
Serial.begin(9600);
Wire.begin();
//Timer1.initialize(); //1 sec period
//Timer1.attachInterrupt(timer_interrupt);
myservo.attach(SERVO_PIN);
myservo.attach(SERVO_PIN);
pos = init_pos;
myservo.write(init_pos);
//send_motor_command('s');
}
void loop() {
escape_safe_space();
nav_spot_one();
nav_spot_two();
fact_check();
return_safe_space();
}
void reload(void){
send_motor_command('r');
move_to_distance(CENTER_LINE_THRESH-15, sonar_L, false);
send_motor_command('b');
move_to_distance(BACK_WALL_THRESH - 5, sonar_B, true);
delay(7500);
}
void return_safe_space(void){
send_motor_command('l');
move_to_distance(CENTER_LINE_THRESH +15, sonar_L, true);
send_motor_command('b');
move_to_distance(BACK_WALL_THRESH - 5, sonar_B, true);
delay(3000);
}
void fact_check(void){
send_motor_command('r');
delay(5000);
fact_checked = true;
}
void nav_spot_two(void){
send_motor_command('f'); //forward
move_to_distance(SPOT_TWO_THRESH, sonar_B, false);
send_motor_command('l');
delay(500);
push_to_wall('x');
shoot();
myservo.write(init_pos);
pos = init_pos;
}
void nav_spot_one(void){
send_motor_command('f'); //forward
move_to_distance(SPOT_ONE_THRESH, sonar_B, false);
send_motor_command('l');
delay(500);
push_to_wall('x');
shoot();
}
void escape_safe_space(void){
int distance = sonar_B.ping_cm();
if(!fact_checked){
send_motor_command('t'); //turn
move_to_distance(BACK_WALL_THRESH, sonar_B, true);
}
send_motor_command('b'); //move back
delay(500);
push_to_wall('y'); //push backwards
delay(500);
send_motor_command('l'); //move left
move_to_distance(WALL_CLOSE_THRESH, sonar_L, true);
send_motor_command('s');
delay(2000);
push_to_wall('x');
}
void send_motor_command(byte code){
Wire.beginTransmission(8); // transmit to device #8
Wire.write(code); // sends one byte
Wire.endTransmission(); // stop transmitting
}
void releaseBall(){
for (int x = pos; x < pos + rotation ; x += 1) {
myservo.write(x);
delay(25);
}
pos += rotation;
}
void shoot(void){
send_motor_command('s');
delay(500);
analogWrite(SHOOT_EN, shooter_speed);
delay(8000);
for(int i = 0; i<3; i++){
releaseBall();
delay(3000);
}
analogWrite(SHOOT_EN , 0);
}
//waits to reach threshold, the returns
void move_to_distance(int thresh, NewPing sonar, bool moving_towards){
int distance = 0;
while(true){
delay(100);
distance = sonar.ping_cm();
//Serial.println(distance);
if(moving_towards){
if((distance != 0) && (distance <= thresh)) {
break;
}
}
else{
if((distance != 0) && (distance >= thresh)) {
break;
}
}
}
}
void push_to_wall(byte code){
send_motor_command(code);
delay(WALL_PAUSE_MS);
}
Motors Code
This code went to the arduino controlling the motors. It reads bytes through I2C from the brain arduino and sets the appropriate motor outputs. The communication between arduinos was handled by the Wire.h library, and by connecting the analog pins of the arduinos.
#include <Wire.h>
//Prototypes
void move_back();
void move_left();
void move_right();
void move_forward();
void turn();
void set_all_motor_output(bool val);
void set_all_enable_output(bool val);
void SetupPins();
void receiveEvent(int howMany);
#define EN_A 5
#define EN_B 6
#define LEFT_1 2
#define LEFT_2 3
#define RIGHT_1 4
#define RIGHT_2 7
#define EN_C 10
#define EN_D 11
#define FRONT_1 8
#define FRONT_2 9
#define BACK_1 12
#define BACK_2 13
#define BREAKING_TIME 1000
#define MOTOR_SPEED 135
#define SPIN_SPEED 90
#define PUSH_SPEED 215
char state = 'n'; //none state
bool serviced = true;
void setup() {
Wire.begin(8); // join i2c bus with address #8
Wire.onReceive(receiveEvent); // register event
Serial.begin(9600); // start serial for output
SetupPins();
set_all_motor_output(false);
}
void loop() {
if(!serviced){
if(state == 't') turn();
else if(state == 'b') move_back();
else if(state == 'l') move_left();
else if(state == 'r') move_right();
else if(state == 'f') move_forward();
else if(state == 'x') push_x_axis();
else if(state == 'y') push_y_axis();
else if(state == 's') stop_all();
serviced = true;
}
}
// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany) {
state = Wire.read();
serviced = false;
}
void turn(){
stop_all();
digitalWrite(LEFT_1, LOW);
digitalWrite(RIGHT_1, HIGH);
digitalWrite(LEFT_2, HIGH);
digitalWrite(RIGHT_2, LOW);
digitalWrite(FRONT_1, LOW);
digitalWrite(BACK_1, HIGH);
digitalWrite(FRONT_2, HIGH);
digitalWrite(BACK_2, LOW);
analogWrite(EN_A, SPIN_SPEED);
analogWrite(EN_B, SPIN_SPEED);
analogWrite(EN_C, SPIN_SPEED);
analogWrite(EN_D, SPIN_SPEED);
}
void move_back(){
stop_all();
digitalWrite(LEFT_1, LOW);
digitalWrite(RIGHT_1, LOW);
digitalWrite(LEFT_2, HIGH);
digitalWrite(RIGHT_2, HIGH);
analogWrite(EN_A, MOTOR_SPEED);
analogWrite(EN_B, MOTOR_SPEED);
}
void move_left(){
stop_all();
digitalWrite(FRONT_1, LOW);
digitalWrite(BACK_1, LOW);
digitalWrite(FRONT_2, HIGH);
digitalWrite(BACK_2, HIGH);
analogWrite(EN_C, MOTOR_SPEED);
analogWrite(EN_D, MOTOR_SPEED);
}
void move_right(){
stop_all();
digitalWrite(FRONT_1, HIGH);
digitalWrite(BACK_1, HIGH);
digitalWrite(FRONT_2, LOW);
digitalWrite(BACK_2, LOW);
analogWrite(EN_C, MOTOR_SPEED);
analogWrite(EN_D, MOTOR_SPEED);
}
void move_forward(){
stop_all();
digitalWrite(LEFT_1, HIGH);
digitalWrite(RIGHT_1, HIGH);
digitalWrite(LEFT_2, LOW);
digitalWrite(RIGHT_2, LOW);
analogWrite(EN_A, MOTOR_SPEED);
analogWrite(EN_B, MOTOR_SPEED);
}
void push_x_axis(){
analogWrite(EN_C, PUSH_SPEED);
analogWrite(EN_D, PUSH_SPEED);
}
void push_y_axis(){
analogWrite(EN_A, PUSH_SPEED);
analogWrite(EN_B, PUSH_SPEED);
}
void stop_all(){
analogWrite(EN_A, 0);
analogWrite(EN_B, 0);
analogWrite(EN_C, 0);
analogWrite(EN_D, 0);
}
void SetupPins(){
pinMode(EN_A, OUTPUT);
pinMode(EN_B, OUTPUT);
pinMode(LEFT_1, OUTPUT);
pinMode(LEFT_2, OUTPUT);
pinMode(RIGHT_1, OUTPUT);
pinMode(RIGHT_2, OUTPUT);
pinMode(EN_C, OUTPUT);
pinMode(EN_D, OUTPUT);
pinMode(FRONT_1, OUTPUT);
pinMode(FRONT_2, OUTPUT);
pinMode(BACK_1, OUTPUT);
pinMode(BACK_2, OUTPUT);
}
void set_all_enable_output(bool val){
if(val){
digitalWrite(EN_A, HIGH);
digitalWrite(EN_B, HIGH);
digitalWrite(EN_C, HIGH);
digitalWrite(EN_D, HIGH);
}
else{
digitalWrite(EN_A, LOW);
digitalWrite(EN_B, LOW);
digitalWrite(EN_C, LOW);
digitalWrite(EN_D, LOW);
}
}
void set_all_motor_output(bool val){
if(val){
digitalWrite(LEFT_1, HIGH);
digitalWrite(LEFT_2, HIGH);
digitalWrite(RIGHT_1, HIGH);
digitalWrite(RIGHT_2, HIGH);
digitalWrite(FRONT_1, HIGH);
digitalWrite(FRONT_2, HIGH);
digitalWrite(BACK_1, HIGH);
digitalWrite(BACK_2, HIGH);
}
else{
digitalWrite(LEFT_1, LOW);
digitalWrite(LEFT_2, LOW);
digitalWrite(RIGHT_1, LOW);
digitalWrite(RIGHT_2, LOW);
digitalWrite(FRONT_1, LOW);
digitalWrite(FRONT_2, LOW);
digitalWrite(BACK_1, LOW);
digitalWrite(BACK_2, LOW);
}
}
//Prototypes
void move_back();
void move_left();
void move_right();
void move_forward();
void turn();
void set_all_motor_output(bool val);
void set_all_enable_output(bool val);
void SetupPins();
void receiveEvent(int howMany);
#define EN_A 5
#define EN_B 6
#define LEFT_1 2
#define LEFT_2 3
#define RIGHT_1 4
#define RIGHT_2 7
#define EN_C 10
#define EN_D 11
#define FRONT_1 8
#define FRONT_2 9
#define BACK_1 12
#define BACK_2 13
#define BREAKING_TIME 1000
#define MOTOR_SPEED 135
#define SPIN_SPEED 90
#define PUSH_SPEED 215
char state = 'n'; //none state
bool serviced = true;
void setup() {
Wire.begin(8); // join i2c bus with address #8
Wire.onReceive(receiveEvent); // register event
Serial.begin(9600); // start serial for output
SetupPins();
set_all_motor_output(false);
}
void loop() {
if(!serviced){
if(state == 't') turn();
else if(state == 'b') move_back();
else if(state == 'l') move_left();
else if(state == 'r') move_right();
else if(state == 'f') move_forward();
else if(state == 'x') push_x_axis();
else if(state == 'y') push_y_axis();
else if(state == 's') stop_all();
serviced = true;
}
}
// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany) {
state = Wire.read();
serviced = false;
}
void turn(){
stop_all();
digitalWrite(LEFT_1, LOW);
digitalWrite(RIGHT_1, HIGH);
digitalWrite(LEFT_2, HIGH);
digitalWrite(RIGHT_2, LOW);
digitalWrite(FRONT_1, LOW);
digitalWrite(BACK_1, HIGH);
digitalWrite(FRONT_2, HIGH);
digitalWrite(BACK_2, LOW);
analogWrite(EN_A, SPIN_SPEED);
analogWrite(EN_B, SPIN_SPEED);
analogWrite(EN_C, SPIN_SPEED);
analogWrite(EN_D, SPIN_SPEED);
}
void move_back(){
stop_all();
digitalWrite(LEFT_1, LOW);
digitalWrite(RIGHT_1, LOW);
digitalWrite(LEFT_2, HIGH);
digitalWrite(RIGHT_2, HIGH);
analogWrite(EN_A, MOTOR_SPEED);
analogWrite(EN_B, MOTOR_SPEED);
}
void move_left(){
stop_all();
digitalWrite(FRONT_1, LOW);
digitalWrite(BACK_1, LOW);
digitalWrite(FRONT_2, HIGH);
digitalWrite(BACK_2, HIGH);
analogWrite(EN_C, MOTOR_SPEED);
analogWrite(EN_D, MOTOR_SPEED);
}
void move_right(){
stop_all();
digitalWrite(FRONT_1, HIGH);
digitalWrite(BACK_1, HIGH);
digitalWrite(FRONT_2, LOW);
digitalWrite(BACK_2, LOW);
analogWrite(EN_C, MOTOR_SPEED);
analogWrite(EN_D, MOTOR_SPEED);
}
void move_forward(){
stop_all();
digitalWrite(LEFT_1, HIGH);
digitalWrite(RIGHT_1, HIGH);
digitalWrite(LEFT_2, LOW);
digitalWrite(RIGHT_2, LOW);
analogWrite(EN_A, MOTOR_SPEED);
analogWrite(EN_B, MOTOR_SPEED);
}
void push_x_axis(){
analogWrite(EN_C, PUSH_SPEED);
analogWrite(EN_D, PUSH_SPEED);
}
void push_y_axis(){
analogWrite(EN_A, PUSH_SPEED);
analogWrite(EN_B, PUSH_SPEED);
}
void stop_all(){
analogWrite(EN_A, 0);
analogWrite(EN_B, 0);
analogWrite(EN_C, 0);
analogWrite(EN_D, 0);
}
void SetupPins(){
pinMode(EN_A, OUTPUT);
pinMode(EN_B, OUTPUT);
pinMode(LEFT_1, OUTPUT);
pinMode(LEFT_2, OUTPUT);
pinMode(RIGHT_1, OUTPUT);
pinMode(RIGHT_2, OUTPUT);
pinMode(EN_C, OUTPUT);
pinMode(EN_D, OUTPUT);
pinMode(FRONT_1, OUTPUT);
pinMode(FRONT_2, OUTPUT);
pinMode(BACK_1, OUTPUT);
pinMode(BACK_2, OUTPUT);
}
void set_all_enable_output(bool val){
if(val){
digitalWrite(EN_A, HIGH);
digitalWrite(EN_B, HIGH);
digitalWrite(EN_C, HIGH);
digitalWrite(EN_D, HIGH);
}
else{
digitalWrite(EN_A, LOW);
digitalWrite(EN_B, LOW);
digitalWrite(EN_C, LOW);
digitalWrite(EN_D, LOW);
}
}
void set_all_motor_output(bool val){
if(val){
digitalWrite(LEFT_1, HIGH);
digitalWrite(LEFT_2, HIGH);
digitalWrite(RIGHT_1, HIGH);
digitalWrite(RIGHT_2, HIGH);
digitalWrite(FRONT_1, HIGH);
digitalWrite(FRONT_2, HIGH);
digitalWrite(BACK_1, HIGH);
digitalWrite(BACK_2, HIGH);
}
else{
digitalWrite(LEFT_1, LOW);
digitalWrite(LEFT_2, LOW);
digitalWrite(RIGHT_1, LOW);
digitalWrite(RIGHT_2, LOW);
digitalWrite(FRONT_1, LOW);
digitalWrite(FRONT_2, LOW);
digitalWrite(BACK_1, LOW);
digitalWrite(BACK_2, LOW);
}
}