PmWiki | Student / WimpmodeCode

 	->Trong Nguyen
	->Blue Cohort
	->Lab 4 Wimpmode
	->November 26

// Include the Command Module API

#include "create.h"

// Pointer to the Sensor Data structure

cr8_t *cr8;

// Declare any new function prototypes here:

uint8_t safe(void); //function for safe mode

// For example: int dashboard(int counter, char partnumber);

// Declare GLOBAL constants used in program:

// Declare GLOBAL Variables used in program:

// Initialize GLOBAL Variables used in program:

// Declaraion for function "main"

int main(void)

{

  ->// Allocate memory for sensor data structure
	->cr8 = cr8_alloc();

  ->/* Initialize the Create and Command Module
     ->Sync communications settings.     */

	->cr8_init(cr8);

  ->// Declare LOCAL constants used in function main:

// For example: const int Pi100 = 314;

  ->// Declare LOCAL Variables used in function main:

	->// For example:  int i, j; 				

  ->// Initialize LOCAL Variables used in function main:

cr8_set_mc_led0(on);

cr8_set_mc_led1(on);

//while the cr8 is in safe mode, set angle to 180 degrees, reset odometer, and poll sensors

while(safe())

	->{ 
    ->cr8_delay(300);
    ->cr8_update(cr8);
	->cr8->dist_mm = 0;

	->while(safe() && (cr8->dist_mm == 0))
		->{//cr8 is waiting for kick with play led on
		->cr8_set_leds(on,off,off,off);
		->cr8_update(cr8);
		->cr8_delay(10);
		->cr8_cricket_display(cr8->dist_mm,0,0);
		->}



    ->while(safe() && cr8->dist_mm <= 600)
        ->{//drive loop while checking for bumps and cliffs
		->cr8_set_leds(on,on,0,255);
		->cr8_drive_direct(200,200);
		->cr8_update(cr8);
		->cr8_cricket_display(cr8->dist_mm,0,0);
		->/*if the left bumper is pushed and the angle is equal or greater than 90 degrees, then
		->perform while*/
		->if(safe() && cr8->bumper_left)

{

           -> cr8_set_leds(off,off,0,255);
			->cr8_update(cr8);
			->cr8->dist_mm = 0;
			->cr8->angle = 0;
			->cr8_drive_direct(200,-200);



			->/*while safe and the left bumper is pushed, reset odometer, 
			->rotate clockwise, then poll sensors*/
			->while(safe() && !cr8->bumper_right
					->&& !cr8->cliff_frontleft && !cr8->cliff_left 
					->&& !cr8->cliff_frontright && !cr8->cliff_right 
					->&& cr8->angle >-90)
				->{
				->cr8_update(cr8);
				->cr8_delay(10);
				->cr8->dist_mm = 0;
				->while(safe() && cr8->bumper_left)
					->{//
					->cr8->dist_mm = 0;
					->cr8->angle = 0;
					->cr8_update(cr8);
					->cr8_delay(10);
					->cr8_cricket_display(cr8->angle,1,0);
					->}
				->cr8_cricket_display(cr8->angle,1,0);
				->}

		   ->}
		->else if(safe() && cr8->bumper_right)
			->{
           ->cr8_set_leds(off,on,off,off);
			->cr8_update(cr8);
			->cr8->dist_mm = 0;
			->cr8->angle = 0;
			->cr8_drive_direct(-200,200);



			->/*while safe and the right bumper is pushed, reset odometer, 
			->rotate counter clockwise, then poll sensors*/
			->while(safe() && !cr8->bumper_left
					->&& !cr8->cliff_frontleft && !cr8->cliff_left 
					->&& !cr8->cliff_frontright && !cr8->cliff_right 
					->&& cr8->angle < 90)
				->{
				->cr8_update(cr8);
				->cr8_delay(10);
				->cr8->dist_mm = 0;
				->while(safe() && cr8->bumper_right)
					->{//
					->cr8->dist_mm = 0;
					->cr8->angle = 0;
					->cr8_update(cr8);
					->cr8_delay(10);
					->cr8_cricket_display(cr8->angle,1,0);
					->}
				->cr8_cricket_display(cr8->angle,1,0);
				->}

		    ->}
         ->else if(cr8->cliff_frontleft || cr8->cliff_left)
			->{
			 ->cr8_drive_direct(-200,-200);
			 ->cr8_delay(300);
			 ->cr8_drive_direct(200,-200);
			 ->//if the angle is greater than or equal to 90, then rotate clockwise
			 ->if(cr8->angle >= 90)
				->{
				 ->cr8_drive_direct(200,-200);
				->}
            ->}	
		  ->else if(cr8->cliff_frontright || cr8->cliff_right)
			->{
			->cr8_drive_direct(-200,-200);
			->cr8_delay(300);
			->cr8_drive_direct(-200,200);
			 ->//if the angle is greater than or equal to 270, then rotate counter clockwise
			 ->if(cr8->angle >= -90)
				->{
				 ->cr8_drive_direct(-200,200);
				->}
			->}
		->}	
	->cr8_drive_direct(0,0);

}

     ->cr8_set_leds(off,off,255,255);
     ->cr8_set_mc_led0(off);
     ->cr8_set_mc_led1(off);
     ->cr8_free(cr8);
     ->return(0);

}

while(safe() && (cr8->angle <= 90))

		->{
		->cr8_update(cr8);
		->cr8_drive_direct(-70,70);
		->}
        ->if(cr8->angle >= 90)
        ->{
		->cr8_drive_direct(0,0);
        ->}





  ->cr8_free(cr8);
  ->return(0);

} // End of function main*/

/* Declare other functions you create here...

For example, to define function xxxxx of type integer (int)

with two input parameters, sample_num and code...

int xxxxx(int sample_num, char code)

{

	->// Declare LOCAL constants used in function xxxxx: 


	->// Declare LOCAL Variables used in function xxxxx: 

	->// Initialize LOCAL Variables ised in function xxxxx: 

	->// Write code for function XXXXX here:




  ->//Return int value for function xxxxx: 
  ->return(1);      You must always return a value to sending program

--in this case a 1 to indicate fn done with no problems.

} // End of function xxxxx

uint8_t safe(void)

{

	->cr8_update(cr8);

	->// Check for unsafe conditions:
	->//    Wheel Drops
	->//    Cliff Detection
	->if(cr8->wheeldrop_left   || cr8->wheeldrop_right ||
	   ->cr8->wheeldrop_caster)
	->{
		->cr8_drive_direct(0,0);
		->cr8_beep_error();
		->cr8_set_leds(off,off,255,255);
		->return 0;
	->}
	->else {
		->// No unsafe conditions found
		->return 1;
	->}

}