• View
• Edit
• History
• Print

TrongsWimpmodeCode

/*

 	Trong Nguyen

	Blue Cohort

	Lab 4 Wimpmode

	November 26

• /

// Include the Command Module API

1. 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;

	}

}