// New version 10/27/2016 for Adafruit Beambreak IR sensors
#include <StopWatch.h>
#include <Keypad.h>
#include <AXE133Y.h>
#include <Servo.h>
StopWatch sw_millis(StopWatch::MILLIS);
volatile long hTime;
volatile int hFlag ;
long rTime;
int rFlag ;
long elapsedTime;
int rCupState = 0;
int hCupState = 0;
int hallInsideState = 0; // variable for reading inside hall status
int hallOutsideState = 0; // variable for reading outside hall status
Servo servoBot; // create servo object to control a bottom firing servo
Servo servoLoad; // create servo object to control a bottom firing servo
const int hallInsidePin = 6; // yellow wire
const int hallOutsidePin = 7; // green wire
const int pinPwm = 5; // Motor PWM is connected to pin 5.
const int pinDir = 4; // Motor rotation DIR is connected to pin 4
const byte ROWS = 3; // Four rows
const byte COLS = 3; // Three columns
// Define the Keymap
char keys[ROWS][COLS] = {
{'1','2','3'},
{'4','5','6'},
{'7','8','9'}
};
// Connect keypad ROW0, ROW1, ROW2 and ROW3 to these Arduino pins.
byte rowPins[ROWS] = { 12, 11, 8 };
// Connect keypad COL0, COL1 and COL2 to these Arduino pins.
byte colPins[COLS] = { 17, 18, 19 };
// Create the Keypad
Keypad kpd = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );
#define ledpin 13
#define oledPin 14
AXE133Y OLED = AXE133Y(oledPin);
void setup()
{ pinMode(2, INPUT);
digitalWrite(2, HIGH); // turn on pullup
pinMode(3, INPUT);
digitalWrite(3,HIGH); //turn on pullup
pinMode(hallInsidePin, INPUT);
pinMode(hallOutsidePin, INPUT);
pinMode(pinPwm, OUTPUT);
pinMode(pinDir, OUTPUT);
pinMode(ledpin,OUTPUT);
pinMode(16, INPUT_PULLUP); // pin 16 (aka A2) is connected to microswitch NC terminals
digitalWrite(ledpin, HIGH); // why is this needed???
Serial.begin(9600);
servoBot.attach(9); // attach firing servo on pin 9 to the servo object
servoLoad.attach(10); // attach reloading servo on pin 10 to the servo object
OLED.clearScreen();
OLED.splash("BOUNCER ROBOT");
}
void loop()
{
char key = kpd.getKey();
if(key) // Check for a valid key.
{
switch (key)
{
case '1':
runRace();
break;
case '2':
digitalWrite(ledpin, HIGH);
break;
case '4':
goOuterMag();
break;
case '5':
goEnd();
break;
case '6':
goInnerMag();
break;
case '7':
fire();
reload();
break;
case '8':
goHome();
break;
case '9':
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print("Get Ready ...");
break;
default:
Serial.println(key);
}
}
}
void runRace(){
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print(" START !!! ");
tone(15,300,500);
hFlag = 0;
rFlag = 0;
rCupState = HIGH;
hCupState = HIGH;
elapsedTime = 0;
rTime = 0 ;
hTime = 0;
sw_millis.reset();
sw_millis.start();
attachInterrupt(digitalPinToInterrupt(3), h_finish, FALLING); // pin d3
// robot shoots 6 balls function call goes here
shoot6();
//elapsedTime = 0;
do{ delay(50);
elapsedTime = sw_millis.elapsed();
Serial.println(elapsedTime);
rCupState = digitalRead(2);
if ((rCupState == LOW) && (rFlag == 0)){
rFlag = 1;
rTime = sw_millis.elapsed();
}
if ((hFlag ==1) && (rFlag ==1)){
break;}
}while(elapsedTime < 30000);
if (hFlag == 0){
hTime = 30000;
detachInterrupt(1);}
if (rFlag ==0){
rTime = 30000; }
sw_millis.stop();
tone(15,300,500);
fdisplay(rTime, hTime);
}
void h_finish() {
Serial.println("interrupt pin3");
hCupState = digitalRead(3);
if ((hCupState == LOW) && (hFlag == 0)){
hFlag = 1;
hTime = sw_millis.elapsed();
detachInterrupt(1);
}
}
void fdisplay(int rtime, int htime) {
if (rtime == htime){
Serial.println(" TIE !!!");
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print(" TIE !!!");
}
else if(rtime < htime){
Serial.println(" ROBOT WINS");
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print(" ROBOT WINS");
}
else {
Serial.println(" HUMAN WINS");
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print(" HUMAN WINS");
}
detachInterrupt(1);
delay(2000);
Serial.print("human time = ");
Serial.println(htime/1000.0);
Serial.print("robot time = ");
Serial.println(rtime/1000.0);
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print("HUMAN Time ");
OLED.printFloat((htime/1000.0),5,2);
OLED.cursorHome(2); //move cusor to position 0 on line 2
OLED.print("ROBOT Time ");
OLED.printFloat((rtime/1000.0),5,2);
}
void fire(){
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print(" FIRE ");
servoBot.write(80); // trigger open -ball fired from chamber
delay(300);
servoBot.write(160); // trigger closed
delay(300);
}
void reload(){
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print(" RELOAD ");
servoLoad.write(80); // Load arm open ball drops into chamber
delay(250);
servoLoad.write(165); // Load arm closes
delay(250);
}
void goHome(){
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print(" Go Home ");
int microSW = digitalRead(16); //read microswitch
while(microSW == LOW){
analogWrite(pinPwm, 255);
digitalWrite(pinDir, LOW); // CCW rotatio pulls sled towards Home
microSW = digitalRead(16);
}
analogWrite(pinPwm, 0); // stop motor
}
void goInnerMag(){
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print("Go Inner Magnet");
hallInsideState = digitalRead(hallInsidePin); //read inside hall sensor
while(hallInsideState == HIGH){
analogWrite(pinPwm, 255);
digitalWrite(pinDir, HIGH); // CC rotatio pulls sled towards End stop
hallInsideState = digitalRead(hallInsidePin);
}
analogWrite(pinPwm, 0); // stop motor
delay(100);
}
void goOuterMag(){
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print("Go Outer Magnet");
hallOutsideState = digitalRead(hallOutsidePin); //read outside hall sensor
while(hallOutsideState == HIGH){
analogWrite(pinPwm, 255);
digitalWrite(pinDir, HIGH); // CC rotatio pulls sled towards End stop
hallOutsideState = digitalRead(hallOutsidePin);
}
analogWrite(pinPwm, 0); // stop motor
delay(100);
}
void goEnd(){
OLED.clearScreen();
OLED.cursorHome(1); //move cusor to position 0 on line 1
OLED.print("Go to End");
hallInsideState = digitalRead(hallInsidePin); //read inside hall sensor
hallOutsideState = digitalRead(hallOutsidePin); //read inside hall sensor
while(hallOutsideState == HIGH or hallInsideState == HIGH){
analogWrite(pinPwm, 255);
digitalWrite(pinDir, HIGH); // CC rotatio pulls sled towards End stop
hallOutsideState = digitalRead(hallOutsidePin);
hallInsideState = digitalRead(hallInsidePin);
}
analogWrite(pinPwm, 0); // stop motor
delay(100);
}
void shoot6(){
fire();
reload();
for (int i=1; i < 3; i++){
goOuterMag();
fire();
reload();
goInnerMag();
fire();
reload();
}
goEnd();
fire();
}
Comments