Frosty the Snowman Game - Arduino Code

//ROOMBA SNOWMAN

//Analog Pins
int speedPot = 0;
int radiusPot = 1;
int timePot = 3;
int flexPin = 5;

//Digital Pins
int goSwitch = 2;
int controlPin = 3;
int upPin = 4;
int downPin = 5;
int wakePin = 8;
int testLedPin = 13;

//Roomba variables
byte speedHi;
byte speedLo;
byte radiusHi;
byte radiusLo;

//Input variables
int go = 0;
int speed = 0;
int radius = 0;
int time = 0;
int currentFlex = 0;
int previousFlex = 0;

int diff = 0;

//Software variables
int currentState = 0;
int lastState = 0;

void setup()
{
//Set pin modes
pinMode(testLedPin, OUTPUT);
pinMode(wakePin,OUTPUT);
pinMode(goSwitch,INPUT);
pinMode(controlPin, OUTPUT);
pinMode(upPin, OUTPUT);
pinMode(downPin, OUTPUT);

Serial.begin(57600);

//Wake Roomba Up
//digitalWrite(wakePin,HIGH);
roombaSetMode();

//Set H-Bridge pin control mode
digitalWrite(controlPin,HIGH);

//Blink LED to show it is starting
blinkLED();
}

void loop ()
{
roombaSetMode();

//Read flex sensor - it determines if snowman should go up or down
//First see if sensor has changed
currentFlex = analogRead(flexPin);
diff = currentFlex - previousFlex;
previousFlex = currentFlex;

//Depending on which way sensor is bent, move snowman up or down by reversing polarity on H-Bridge
if (diff > 10){
digitalWrite(upPin,HIGH);
digitalWrite(downPin,LOW);
}
else if (diff < -10){
digitalWrite(upPin,LOW);
digitalWrite(downPin,HIGH);
}
//Otherwise stop it
else{
digitalWrite(upPin,LOW);
digitalWrite(downPin,LOW);
}

//Check state of the go Switch - set flag to go or not
currentState = digitalRead(goSwitch);

if (currentState == 1){
if (lastState == 0){
go = 1;
}
}
lastState = currentState;

//If go button was pressed - read pots and move Roomba
if (go == 1){

//Read speed pot - normalize to fit Roomba values (0 to 500) only going forward here
speed = ((analogRead(speedPot) * 10)/1024) * (500/10) + 50;

//Read time pot - determines how long Roomba will move for
time = analogRead(timePot) * 20 + 100;

//Read radius pot - determines how tight it will turn
radius = analogRead(radiusPot);

//Serial.print("Pot Value = ");
//Serial.println(radius);

//Normalize radius values so that center position of pot makes Roomba go straight
if (radius > 500 && radius < 520){
radius = 32768;
}
else if (radius > 520){
radius = 4100 - (radius * 4);
}
else{
radius = - (radius * 4);
}

if (radius > 2000){
radius = 2000;
}
else if (radius < -2000){
radius = -2000;
}

//Serial.print("Radius = ");
//Serial.println(radius);

//Move the Roomba
roombaMove(speed, radius);

//Keep it moving for the specified amount of time from time pot
delay(time);

//Stop Roomba
roombaMove(0,32768);
song();
go = 0;
}
}

void roombaMove(int s, int r)
{
speedHi = s >> 8;
speedLo = s & 255;
radiusHi = r >> 8;
radiusLo = r & 255;

if (s < 0){
speedHi = speedHi & 32767;
speedLo = speedLo & 32767;
}
if (r < 0){
radiusHi = radiusHi & 32767;
radiusLo = radiusLo & 32767;
}

Serial.print(137, BYTE); //Roomba Drive command
Serial.print(speedHi, BYTE); //Hi Byte speed value
Serial.print(speedLo, BYTE); //Low Byte speed value
Serial.print(radiusHi, BYTE); //Hi byte radius value
Serial.print(radiusLo, BYTE); //Low byte radius
}
void roombaSetMode()
{
digitalWrite(wakePin, LOW); // Wake it up
delay(500);
digitalWrite(wakePin, HIGH);

Serial.print(128,BYTE); //Start Roomba & puts it in passive mode
// delay(50);
Serial.print(130,BYTE); //Enables control of the Roomba
//delay(50);
}

void blinkLED()
{
digitalWrite(testLedPin, HIGH);
delay(200);
digitalWrite(testLedPin, LOW);
delay(200);
}
void song()
{
//Command
Serial.print(140,BYTE);
//Song number
Serial.print(1,BYTE);
//Song length
Serial.print(21,BYTE);

//1st quarter 1
Serial.print(71,BYTE);
Serial.print(10,BYTE);
//Pause 2
Serial.print(30,BYTE);
Serial.print(10,BYTE);
//Note 3
Serial.print(71,BYTE);
Serial.print(10,BYTE);
//Pause 4
Serial.print(30,BYTE);
Serial.print(10,BYTE);
//Note 5
Serial.print(71,BYTE);
Serial.print(20,BYTE);
//Pause 6
Serial.print(30,BYTE);
Serial.print(20,BYTE);

//2nd quarter 7
Serial.print(71,BYTE);
Serial.print(10,BYTE);
//Pause 8
Serial.print(30,BYTE);
Serial.print(10,BYTE);
//Note 9
Serial.print(71,BYTE);
Serial.print(10,BYTE);
//Pause 10
Serial.print(30,BYTE);
Serial.print(10,BYTE);
//Note 11
Serial.print(71,BYTE);
Serial.print(20,BYTE);
//Pause 12
Serial.print(30,BYTE);
Serial.print(20,BYTE);

//3rd quarter 13
Serial.print(71,BYTE);
Serial.print(10,BYTE);
//Pause 14
Serial.print(30,BYTE);
Serial.print(10,BYTE);
//Note 15
Serial.print(74,BYTE);
Serial.print(10,BYTE);
//Pause 16
Serial.print(30,BYTE);
Serial.print(10,BYTE);
//Note 17
Serial.print(67,BYTE);
Serial.print(10,BYTE);
//Pause 18
Serial.print(30,BYTE);
Serial.print(10,BYTE);
//Note 19
Serial.print(69,BYTE);
Serial.print(10,BYTE);
//Pause 20
Serial.print(30,BYTE);
Serial.print(10,BYTE);

//4th quarter 21
Serial.print(71,BYTE);
Serial.print(80,BYTE);

delay(10);
//Play
Serial.print(141,BYTE);
Serial.print(1,BYTE);
}