/*--------------- Includes ---------------*/

#include <Timers.h>
#include <Servo.h>
#include <Ultrasonic.h>
/*---------------Module Defines---------------*/
#define LiftingTime
5000
#define DumpingTime
10000
#define TURN_BUFFER
100
#define QTR_SEC
250
#define HALF_SEC
500
#define ONE_SEC
1000
#define TWO_MINUTES
120000
#define TIME_INTERVAL
TWO_MINUTES
#define SPEED_SCALAR
25
#define SERVER_PERIOD
1.17647 // 850 Hz period
#define GameTimer
1
#define AreYouBeaconTimer
2
#define ServerBeaconTimer
3
#define TurntoTapeTimer
4
#define FaceServerTimer
5
#define PunchTimer
6
#define RetractTimer
7
#define IsItHitTimer
8
#define DumpingTimer
9
#define LiftDumperTimer
10
int
int
int
int
int
int

ECpin1 = A1; // Front EC pin

ECpin2 = A3; // Back Middle EC pin
ECpin3 = A0; // Back Left EC pin
ECpin4 = A2; // Back Right EC pin
ECvalue1, ECvalue2, ECvalue3, ECvalue4;
LightThreshold=1000; // greater than 1000 = white

int PTpin1=8; // Front PT

int PTpin2=13; // Back PT
//int Proxpin1=9; // Front Left Prox
//int Proxpin2=7; // Front Right Prox
long Proxvalue1,Proxvalue2;
long OldProxvalue1, OldProxvalue2;
Ultrasonic ultrasonic1(9);
Ultrasonic ultrasonic2(7); // might need to be switched
int PunchButtonPin=7;
int CoinBucketPin=8;
int MiddleDistance=40;
int CollectingDistance=5;
int DepositingDistance=89;
int
int
int
int
int

LMtrEnPin=5; // analog
LMtrDirPin=4; // digital
RMtrEnPin=3; // analogWrite
RMtrDirPin=2; //digitalWrite
MtrSpeed;

int PuncherEnPin=11; // analogWrite

int PuncherDirPin=12; // digitalWrite

Servo lifter;
int LifterPin=10; // servo
int dumpposition=130;
int collectposition=65;
int
int
int
int
int
int

ServerBeaconThreshold1=1130;
ServerBeaconThreshold2=1180;
ExchangeThreshold1=310;
ExchangeThreshold2=350;
FrontTapeThreshold=1000;
BackTapeThreshold=400;

unsigned int beaconstate;

int BeaconFound;
int CoinNumber=1;
int NumHits=0;
unsigned int state;
typedef enum
{
FindingServer = 0,
AlignedtoServerWall = 1,
ApproachingServer = 2,
Collecting = 3,
FindingExchange8 = 4,
ApproachingExchange8 = 5,
Depositing = 6,
GameOver=7,
Right=8,
Left=9,
};
/*--------------- Module Function Prototypes ---------------*/
void LeftMtrSpeed(int);
void RightMtrSpeed(int);
void ForwardBrake();
void BackwardBrake();
unsigned char FindServerBeacon(void);
void AligntoServerWall(unsigned int &beaconstate);
void AligntoServer(void);
void ApproachServerBeacon(void);
void CollectCoins(int &CoinNumber, int &NumHits);
unsigned char FindExchangeBeacon(void);
void ApproachExchangeBeacon(void);
void DepositCoins(void);
void EndGame(void);
/*--------------- Running Code ---------------*/
void setup()
{
Serial.begin(9600);
pinMode(LMtrEnPin,OUTPUT);
pinMode(LMtrDirPin,OUTPUT);
pinMode(RMtrEnPin,OUTPUT);
pinMode(RMtrDirPin,OUTPUT);
pinMode(PuncherEnPin,OUTPUT);

pinMode(PuncherDirPin,OUTPUT);
lifter.attach(LifterPin);
lifter.write(collectposition);
digitalWrite(PuncherDirPin,LOW);
analogWrite(PuncherEnPin,255);
state=FindingServer;
TMRArd_InitTimer(GameTimer,5000);//TIME_INTERVAL);
}
void loop()
{
if(TMRArd_IsTimerExpired(GameTimer))
{
state=GameOver;
}
switch (state)
{
case (FindingServer):

case (AlignedtoServerWall):
case (ApproachingServer):
case (Collecting):
case (FindingExchange8):
case (ApproachingExchange8):
case (Depositing):
case (GameOver):

if(FindServerBeacon()==1)
{
AligntoServerWall(beaconstate);
}
break;
AligntoServer();
break;
ApproachServerBeacon();
break;
CollectCoins(CoinNumber,NumHits);
break;
break; // Do we need this for Friday?
ApproachExchangeBeacon();
break;
DepositCoins();
break;
EndGame();
break;

}
Serial.println(state);
}
/*--------------- Module Functions ---------------*/
void LeftMtrSpeed(int newSpeed)
{
if (newSpeed < 0)
{
digitalWrite(LMtrDirPin,LOW);
analogWrite(LMtrEnPin,abs(newSpeed+6));
}
else if(newSpeed>0)
{
digitalWrite(LMtrDirPin,HIGH);
analogWrite(LMtrEnPin,abs(newSpeed-2));
}
else
{
digitalWrite(LMtrDirPin,HIGH);
analogWrite(LMtrEnPin,newSpeed);
}

}
void RightMtrSpeed(int newSpeed)
{
if (newSpeed < 0)
{
digitalWrite(RMtrDirPin,HIGH);
}
else
{
digitalWrite(RMtrDirPin,LOW);
}
analogWrite(RMtrEnPin,abs(newSpeed));
}
void ForwardBrake()
{
LeftMtrSpeed(-248);
delay(15);
RightMtrSpeed(-255);
delay(50);
LeftMtrSpeed(0);
RightMtrSpeed(0);
delay(1000);
}
void BackwardBrake()
{
LeftMtrSpeed(248);
delay(20);
RightMtrSpeed(255);
delay(50);
LeftMtrSpeed(0);
RightMtrSpeed(0);
delay(1000);
}
unsigned char FindServerBeacon(void)
{
unsigned char BeaconReturn=0;
static int timerstate=0;
if(BeaconFound==1)
{
BeaconReturn=1;
Serial.println("found beacon");
}
else // if 850 Hz timer is not active
{
int duration1=pulseIn(PTpin1,HIGH);
int duration2=pulseIn(PTpin1,LOW);
int duration=duration1+duration2;
Serial.println(duration);
if (timerstate==0 && (duration < ServerBeaconThreshold1 or duration > Server

BeaconThreshold2) )
{
LeftMtrSpeed(-115);
RightMtrSpeed(115);
//
//
delay(250);
//
//
LeftMtrSpeed(0);
//
RightMtrSpeed(0);
//
delay(100);
}
else if(timerstate==0 && duration > ServerBeaconThreshold1 && duration < Ser
verBeaconThreshold2)
{
Serial.println("if beaconfound");
LeftMtrSpeed(0);
RightMtrSpeed(0);
delay(500);
LeftMtrSpeed(150);
delay(20);
RightMtrSpeed(140);
delay(4500);
// Stop when one prox is <20, whichever one is <20 determi
nes where u are oriented
ForwardBrake();
ultrasonic1.DistanceMeasure();
delay(10);
ultrasonic2.DistanceMeasure();
delay(10);
Proxvalue1=ultrasonic1.microsecondsToCentimeters(); // Front Left Prox pin
Proxvalue2=ultrasonic2.microsecondsToCentimeters(); // Front Right Prox pi
n
Serial.print("Front Left value:
");
Serial.println(Proxvalue1);
Serial.print("Front Right value:
");
Serial.println(Proxvalue2);
if(Proxvalue1 < 20) // If left front prox is closer than right front prox,
then beacon is on right
{
//
if(Proxvalue2 > 100)
//
{
//
Serial.println("beacon on left");
//
beaconstate=Left;
//
LeftMtrSpeed(155);
//
RightMtrSpeed(-155);
//
delay(250);
//
}
//
else
//
{
Serial.println("beacon on right");
beaconstate=Right;
LeftMtrSpeed(-155);

//

RightMtrSpeed(-155);
delay(2500);
}

}
else // else beacon is on left
{
//
if(Proxvalue1>100)
//
{
//
Serial.println("beacon on right");
//
beaconstate=Right;
//
//LeftMtrSpeed(-155);
//
//RightMtrSpeed(155);
////
delay(15);
//
}
//
else
//
{
Serial.println("beacon on left");
beaconstate=Left;
LeftMtrSpeed(-155);
RightMtrSpeed(-155);
delay(2500);
//
}
}
BeaconFound=1; // Beacon is Found!
BeaconReturn=1;
}
}
return BeaconReturn;
}
void AligntoServerWall(unsigned int &beaconstate)
{
static int tape=0;
ECvalue1=analogRead(ECpin1);
if(ECvalue1<FrontTapeThreshold)
{
//stop

//
//
//
//
//
//

//
//
//
//
//

if(beaconstate==Right)
{
LeftMtrSpeed(-215);
RightMtrSpeed(-160);
delay(50);
LeftMtrSpeed(0);
RightMtrSpeed(0);
delay(500);
ForwardBrake();
delay(150);
}
else if (beaconstate==Left)
{
LeftMtrSpeed(-185);
RightMtrSpeed(-160);
delay(50);
LeftMtrSpeed(0);
RightMtrSpeed(0);

//

delay(500);
ForwardBrake();
delay(150);
}
state=AlignedtoServerWall; // should be state=AlignedtoServerWall
tape=1;
}
if(tape==0)
{
if(beaconstate==Right)
{
LeftMtrSpeed(215);
RightMtrSpeed(160);
}
else if(beaconstate==Left)
{
LeftMtrSpeed(160);
RightMtrSpeed(195);
}
}

}
void AligntoServer(void)
{
ECvalue1=analogRead(ECpin1);
ECvalue2=analogRead(ECpin2);
ultrasonic1.DistanceMeasure();
delay(10);
ultrasonic2.DistanceMeasure();
delay(10);
Proxvalue1=ultrasonic1.microsecondsToCentimeters(); // Front Left Prox pin
Proxvalue2=ultrasonic2.microsecondsToCentimeters(); // Front Right Prox pin
if((Proxvalue1+3==Proxvalue2) && Proxvalue1 <45 ) //and (Proxvalue1 <= Proxval
ue2+1 && Proxvalue1 >=Proxvalue2-1)) //
{
LeftMtrSpeed(0);
RightMtrSpeed(0);
state=ApproachingServer;
delay(1000);
}
else
{
ECvalue1=analogRead(ECpin1);
ECvalue2=analogRead(ECpin2);
static int stupidtape=0;
if(ECvalue1>=FrontTapeThreshold and stupidtape==0)
{
LeftMtrSpeed(-155);
RightMtrSpeed(-155);
delay(250);

BackwardBrake();
delay(100);
}
else if(ECvalue1<FrontTapeThreshold)
{
stupidtape=1;
if(beaconstate==Left)
{
LeftMtrSpeed(130);
RightMtrSpeed(-128);
//
//
//
//
//
//
//

delay(250);
LeftMtrSpeed(0);
RightMtrSpeed(0);
delay(100);
}
else if (beaconstate==Right)
{
LeftMtrSpeed(-130);
RightMtrSpeed(124);
//
//
//
//
//
//
//

delay(250);
LeftMtrSpeed(0);
RightMtrSpeed(0);
delay(100);
}
}

}
}
void ApproachServerBeacon(void)
{
ultrasonic1.DistanceMeasure();
delay(10);
ultrasonic2.DistanceMeasure();
delay(10);
Proxvalue1=ultrasonic1.microsecondsToCentimeters(); // Front Left Prox pin
Proxvalue2=ultrasonic2.microsecondsToCentimeters(); // Front Right Prox pin
if(Proxvalue2 < CollectingDistance)
{
ForwardBrake();
state=Collecting;
}
else
{
LeftMtrSpeed(170);
delay(20);
RightMtrSpeed(150);

}
}
void CollectCoins(int &CoinNumber, int &NumHits)
{
/*FOR NO BUTTON */
Serial.println("Punching that Mofo");
if (TMRArd_IsTimerExpired(PunchTimer))
{
LeftMtrSpeed(155);
delay(20);
RightMtrSpeed(155);
digitalWrite(PuncherDirPin,HIGH);
analogWrite(PuncherEnPin,255);
delay(100);
digitalWrite(PuncherDirPin,LOW);
analogWrite(PuncherEnPin,255);
delay(500);
NumHits++;
if(NumHits == CoinNumber)
{
NumHits=0;
CoinNumber++;
}
if(CoinNumber == 9)
{
LeftMtrSpeed(-155);
RightMtrSpeed(-155);
delay(500);
LeftMtrSpeed(155);
delay(20);
RightMtrSpeed(155);
delay (800);
//
//
//
//
//
//
//
//
//
//
//

BackwardBrake();
LeftMtrSpeed(-170);
delay(20);
RightMtrSpeed(-155);
delay(2000);
LeftMtrSpeed(-160);
delay(20);
RightMtrSpeed(-145);
state=ApproachingExchange8;

}
}
/*
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//

FOR BUTTON */
if (TMRArd_IsTimerActive(PunchTimer)==0)
{
NumHits=0;
digitalWrite(PuncherDirPin,HIGH);
analogWrite(PuncherEnPin,255);
TMRArd_InitTimer(PunchTimer,HALF_SEC);
TMRArd_InitTimer(IsItHitTimer,QTR_SEC);
}
else if (TMRArd_IsTimerActive(PunchTimer))
{
if (TMRArd_IsTimerExpired(IsItHitTimer))
{
int Hit;
Hit = digitalRead(PunchButtonPin);
if(Hit==1)
{
NumHits++;
TMRArd_InitTimer(RetractTimer,QTR_SEC);
if(NumHits >= CoinNumber)
{
CoinNumber++;
NumHits=0;
}
}
else
{
digitalWrite(PuncherDirPin,LOW); // retract
analogWrite(PuncherEnPin,255);
TMRArd_InitTimer(PunchTimer,QTR_SEC);
TMRArd_InitTimer(IsItHitTimer,ONE_SEC);
}
}
if (TMRArd_IsTimerExpired(RetractTimer))
{
digitalWrite(PuncherDirPin,LOW);
analogWrite(PuncherEnPin,255);
}
else if(TMRArd_IsTimerExpired(PunchTimer) && NumHits < CoinNumber)
{
digitalWrite(PuncherDirPin,HIGH);
analogWrite(PuncherEnPin,255);
TMRArd_InitTimer(PunchTimer,HALF_SEC);
TMRArd_InitTimer(IsItHitTimer,QTR_SEC);
}
}
if(CoinNumber==9)
{
digitalWrite(PuncherEnPin,LOW);
state=FindingExchange8;
}

}
unsigned char FindExchangeBeacon(void)
{
}
void ApproachExchangeBeacon(void)
{
Serial.println("Approaching Exchange");
ultrasonic1.DistanceMeasure();
delay(10);
ultrasonic2.DistanceMeasure();
delay(10);
Proxvalue1=ultrasonic1.microsecondsToCentimeters(); // Front Left Prox pin
Proxvalue2=ultrasonic2.microsecondsToCentimeters(); // Front Right Prox pin
Serial.print("Front Left value:
Serial.println(Proxvalue1);
Serial.print("Front Right value:
Serial.println(Proxvalue2);

");
");

if(Proxvalue1 >= DepositingDistance or Proxvalue2 >= DepositingDistance)

{
BackwardBrake();
state=Depositing;
}
else
{
LeftMtrSpeed(-175);
RightMtrSpeed(-153);
}
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//

static int exchangebeaconfound=0;

if (exchangebeaconfound=1)
{
Serial.println("Approaching Exchange");
ultrasonic1.DistanceMeasure();
delay(10);
ultrasonic2.DistanceMeasure();
delay(10);
Proxvalue1=ultrasonic1.microsecondsToCentimeters(); // Front Left Prox pin
Proxvalue2=ultrasonic2.microsecondsToCentimeters(); // Front Right Prox pin
Serial.print("Front Left value:
Serial.println(Proxvalue1);
Serial.print("Front Right value:
Serial.println(Proxvalue2);

");
");

if(Proxvalue1 >= DepositingDistance or Proxvalue2 >= DepositingDistance)

{
BackwardBrake();

//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//
//

state=Depositing;
}
else
{
LeftMtrSpeed(-155);
RightMtrSpeed(-153);
}
}
else
{
int duration1=pulseIn(PTpin2,HIGH);
int duration2=pulseIn(PTpin2,LOW);
int duration=duration1+duration2;
Serial.println(duration);
if ((duration < ExchangeThreshold1 or duration > ExchangeThreshold2) )
{
LeftMtrSpeed(-155);
RightMtrSpeed(155);
delay(250);
LeftMtrSpeed(0);
RightMtrSpeed(0);
delay(100);
}
}

}
void DepositCoins(void)
{
lifter.write(dumpposition);
delay(5000);
lifter.write(collectposition);
delay(3000);
state=GameOver;
}
void EndGame(void)
{
static int gameover=0;
if(gameover=0)
{
LeftMtrSpeed(160);
RightMtrSpeed(160);
gameover=1;
delay(2500);
}
else
{
LeftMtrSpeed(-200);

RightMtrSpeed(200);
lifter.write(dumpposition);
delay(5000);
LeftMtrSpeed(200);
RightMtrSpeed(-200);
lifter.write(collectposition);
delay(5000);
}
}