MarkLab4Code

/* 
   Wimp Mode by Mark Troutt
*/


// Include the Command Module API
#include "create.h"

// Pointer to the Sensor Data structure
cr8_t *cr8;

// Declare any new function prototypes:
int safe(void);
void turn(int right, int time);
void updateMusic(void);
void backUp(void);

int song_time = 0, song = 0;
int playlist[] = { 4, 5, 4, 5, 0, 1, 2, 3 };
int playtime[] = {6000, 1750, 6000, 1750, 6000, 6000, 6000, 6000 };

// Main function
int main(void)
{
  // Allocate memory for sensor data structure
	cr8 = cr8_alloc();

  // Initialize the Create and Command Module
  // Sync communications settings.
	cr8_init(cr8);

  //Code goes here

  int moving = 0; //variable to tell us when we're moving

  //stores all the music:
  int sing[] = { 140, 0, 13, 64, 32, 65, 16, 67, 16, 72, 64, //start Indy
					30, 16, 62, 32, 64, 16, 65, 64, 30, 16, 67,
					32, 69, 16, 71, 16, 77, 48,
				  140, 1, 14, 69, 32, 71, 16, 72, 32, 74, 32,
					76, 32, 30, 16, 64, 32, 65, 16, 67, 16, 72, 
					64, 30, 16, 74, 32, 76, 16, 77, 32,
				  140, 2, 12, 67, 32, 67, 16, 76, 48, 74, 16,
					67, 16, 76, 48, 74, 16, 67, 16, 77, 48,
					76, 32, 74, 16, 72, 64,
				  140, 3, 6, 30, 64, 30, 64, 30, 64, 30, 64,
					30, 64, 30, 64,							   //end Indy
				  140, 4, 15, 65, 16, 65, 16, 65, 16, 70, 48,   //start SW
					77, 48, 75, 16, 74, 16, 72, 16, 82, 48, 77, 32,
					75, 16, 74, 16, 72, 16, 82, 48, 77, 32,
				  140, 5, 4, 75, 16, 74, 16, 75, 16, 72, 64,
				  140, 15, 4, 65, 16, 65, 16, 69, 16, 65, 32};

  //executes the commands from sing[]
  for(int i=0; i < 157; i++)
  {
    cr8_byte_tx( sing[i] ); //sends a byte to the create
  }

  cr8_stopwatch_start();

  cr8->dist_mm = 0;  //zero out the distance

  cr8_set_leds(1, 0, 0, 255); //turn on advance and play LEDs

  while(!cr8->button_play){ //waits for play button before starting
    cr8_update(cr8);
  }

  /*cr8_byte_tx( 141 ); //these two lines play the initial start beeps
  cr8_byte_tx( 15 );*/

  while(safe()) //main loop
  {
	while(safe() && moving != 0) //moving loop
	{
		cr8_update(cr8);
		if(cr8->bumper_right) //turns if right bumper is hit
		{
			cr8_stopwatch_stop();
			cr8_stopwatch_start();
			turn(true, 12);
			cr8_stopwatch_start();
			moving = 1;
			cr8_set_mc_led0(1);
			cr8_set_mc_led1(0);
		}
		else if(cr8->bumper_left) //turns if left bumper is hit
		{
			cr8_stopwatch_stop();
			cr8_stopwatch_start();
			turn(false, 12);
			cr8_stopwatch_start();
			moving = 1;
			cr8_set_mc_led0(0);
			cr8_set_mc_led1(1);
		}
		else
		{
			if(moving == 1) //Starts create moving
			{
				cr8_drive_direct(200, 200);
				moving = 2;
				cr8_set_mc_led0(1);
				cr8_set_mc_led1(1);
				cr8_stopwatch_start();
				song_time += 26;
			}
			else if(cr8_stopwatch_running() && cr8_stopwatch_time() > 30) //Stops create
			{
				song_time += 26;
				cr8_drive_direct(0, 0);
				moving = 0;
				cr8_set_mc_led0(0);
				cr8_set_mc_led1(0);
				cr8_stopwatch_stop();
			}
		}
		song_time += 26;
		updateMusic();
	}

	//makes sure important values are reset
	cr8_delay(250);
	song_time += 250;
	cr8_update(cr8);
	cr8_stopwatch_stop();
	cr8->dist_mm = 0;
	moving = 1;

	//this loop waits for the create to be pushed
	while(safe() && cr8->dist_mm == 0){
		cr8_update(cr8);
		song_time += 26;
		updateMusic();
	}
  }

  cr8_set_leds(0, 0, 0, 0); //turn off create lights
  cr8_set_mc_led0(0);       //turn off command module lights
  cr8_set_mc_led1(0);
  cr8_drive_direct(0, 0);   //stop moving

  //End main Code


  // Release the sensor data structure memory and exit the program
  cr8_free(cr8);
  return 0;
}  // End of function main

// safe() checks if create is safe and returns status.
// If create is at a cliff it backs up.
int safe(void){
  int x = true;
  if(cr8->wheeldrop_right || cr8->wheeldrop_left ||
    cr8->wheeldrop_caster )
  {
    x = false;
	cr8_drive_direct(0, 0);
  }
  else if(cr8->cliff_left || cr8->cliff_right || cr8->cliff_frontleft ||
		  cr8->cliff_frontright){
	cr8_stopwatch_stop();
	cr8_stopwatch_start();
	backUp();
	cr8_stopwatch_start();
	song_time += 26;
  }
  return x;
}  //end safe

// turn(int right) turns create in one direction or another
// depending on the information it recieves.  Returns nothing.
void turn(int right, int time){
	if(!right)
	{
		cr8_drive_direct(200, -200);
		while(cr8_stopwatch_time() < time && safe()) 
		{
			cr8_update(cr8);
			song_time += 26;
			updateMusic();
		}
		cr8_drive_direct(0, 0);
	}
	else
	{
		cr8_drive_direct(-200, 200);
		while(cr8_stopwatch_time() < time && safe())
		{
			cr8_update(cr8);
			song_time += 26;
			updateMusic();
		}
		cr8_drive_direct(0, 0);
	}
	cr8_stopwatch_stop();
	return;
}  //end turn

// backUp() gets the create away from a cliff
void backUp(void){
	cr8_drive_direct(-200, -200);
	while(cr8_stopwatch_time() < 6 && safe())
	{
		cr8_update(cr8);
		song_time += 26;
		updateMusic();
	}
	cr8_stopwatch_stop();
	cr8_stopwatch_start();
	turn(true, 6);
	cr8_stopwatch_start();
	song_time += 26;

	return;
}  //end backUp

// updateMusic() checks to see if a song is done and
// plays the next one if it is.  Returns nothing.
void updateMusic(void){
	if(song_time > playtime[song % 8]){// 6000){
		cr8_byte_tx(141);
		cr8_byte_tx(playlist[song % 8]);
		song_time = 0;
		song++;
	}
	return;
}  //end updateMusic