void CBOT_main( void )
{
// Initialize variables
int btnValue=0;//value of button pushed
ATopstat = ATTINY_open();//open the tiny microcontroller
LEopstat = LED_open(); //open the LED module
LCopstat = LCD_open(); //open the LCD module
STEPPER_open(); // Open STEPPER module for use
SPKR_open(SPKR_BEEP_MODE);//open the speaker in beep mode
LED_open();
I2C_open();
ADC_open();//open the ADC module
ADC_set_VREF( ADC_VREF_AVCC );// Set the Voltage Reference first so VREF=5V.
// Initialize IR Values and Reset Prefilter
checkIR();
prefilter(1);
// LCD_printf("PRESS a button\nOR\nWAIT for default\n");
//TMRSRVC_delay(3000);//wait 3 seconds
//btnValue = WaitButton();
LCD_clear;
// Infinite loop
while (1)
{
// update the sensor values
checkLightSensor();
checkIR();
checkContactIR();
//Test contact Sensors
// LCD_printf("Right Contact: %i\nLeft Contact: %i\n\n\n",rightContact,leftContact);
// TMRSRVC_delay(1000);//wait 1 seconds
//Test IR Sensors
// LCD_printf("FrontIR = %3.2f\nBackIR = %3.2f\nLeftIR = %3.2f\nRightIR = %3.2f\n", ftIR,bkIR,ltIR,rtIR);
// TMRSRVC_delay(2000);//wait 2 seconds
// run the moveBehavior FSM
moveBehavior(LIGHT_LOVER);
// debug primitive behaviors
// moveAway();
// moveWall();
// moveRetreat();
// moveTrackLight();
// moveWander();
}
}// end the CBOT_main()
/*******************************************************************
* Function: void moveAway(void)
* Input Variables: none
* Output Return: none
* Overview: Use a comment block like this before functions
********************************************************************/
void moveAway ( void )
{
checkIR();
if ((ftIR < IR_THRESHOLD)|(bkIR < IR_THRESHOLD)|(ltIR < IR_THRESHOLD)|(rtIR < IR_THRESHOLD))
{
float move = bkIR - ftIR;
BOOL moveForward = move >= 0;
float turn = ltIR - rtIR;
BOOL turnCW = turn >= 0;
if(moveForward == 1){
turn = -turn;
}
// Move.
STEPPER_move_stnb( STEPPER_BOTH,
moveForward, (move)/10, 200+turn+move, 450, STEPPER_BRK_OFF, // Left
moveForward, (move)/10, 200-turn+move, 450, STEPPER_BRK_OFF ); // Right
}
else
{
//STOP.
STEPPER_move_stnb( STEPPER_BOTH,
STEPPER_FWD, 0, 0, 0, STEPPER_BRK_OFF, // Left
STEPPER_FWD, 0, 0, 0, STEPPER_BRK_OFF ); // Right
}
}
/*******************************************************************
* Function: void initializeRobot(void)
* Input Variables: none
* Output Return: none
* Overview: This initialize the robot by using other startups
********************************************************************/
void initializeRobot(void)
{
ATopstat = ATTINY_open();//open the tiny microcontroller
LEopstat = LED_open(); //open the LED module
LCopstat = LCD_open(); //open the LCD module
STEPPER_open(); // Open STEPPER module for use
SPKR_open(SPKR_TONE_MODE);//open the speaker in tone mode
LED_open();
I2C_open();
ADC_open();//open the ADC module
ADC_set_VREF( ADC_VREF_AVCC );// Set the Voltage Reference first so VREF=5V.
// Initialize IR Values and Reset Prefilter
checkIR();
prefilter(1);
// Mistake? odometryTrigger = WORLD_RESOLUTION_SIZE*D_STEP which is about 6
odometryTrigger = WORLD_RESOLUTION_SIZE/2.75;
// pixel array for the LCD screen
for(int i = 0; i < 4; i++) {
for(int j = 0; j < 32; j++) {
pix_arr[i][j] = 0x00;
}
}
}
/*******************************************************************
* Function: void map (void)
* Input Variables: none
* Output Return: none
* Overview: Makes the robot map the world
********************************************************************/
void map (void)
{
// Initialize State
isMapping = 1;
// Mapping Loop
while(isMapping)
{
//Sense
checkIR();
checkWorld();
//Record
setGateways();
//Plan using the Map
planMap();
//Act on the Map
moveMap();
//Shift the Map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
//Break?
isMapping = !((currentCellWorldStart == currentCellWorld)&&(currentOrientationStart == currentOrientation));
if(!isMapping){
break;
}
//Print Map
LCD_clear();
LCD_printf(" Move"BYTETOBINARYPATTERN"\n Cell"BYTETOBINARYPATTERN"\n Ornt"BYTETOBINARYPATTERN"\n\n",BYTETOBINARY(currentMove),BYTETOBINARY(currentCellWorld),BYTETOBINARY(currentOrientation));
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(500);//wait 3 seconds
}
}
int main(void) {
out(DEBUGLED);
for(uint8_t i=0; i<5; i++) {
toggle(DEBUGLED);
_delay_ms(100);
}
off(DEBUGLED);
bt_init();
irmp_init();
timer1_init();
pwm_init();
IRMP_DATA irmp_data;
sei();
// polling mode
while(1) {
checkIR(&irmp_data);
checkBT();
}
}//end main
/*******************************************************************
* Function: void moveMap(void)
* Input Variables: void
* Output Return: void
* Overview: moves the robot through the map
********************************************************************/
void moveMap( void )
{
char isDone = 0;
pidController(0,RESET);
switch(currentMove){
case MOVE_LEFT:
move_arc_stwt(POINT_TURN, LEFT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
case MOVE_FORWARD:
setOdometry(WALL_STEP);
while(!isDone){
checkIR();
isDone = moveWall();
}
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
// move_arc_stwt(NO_TURN, 45, 10, 10, 0);
break;
case MOVE_RIGHT:
move_arc_stwt(POINT_TURN, RIGHT_TURN, 30, 30, 0);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_ON);
TMRSRVC_delay(BRAKE_DELAY);
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
break;
default:
LCD_printf("Whatz2?!");
break;
}
}
task main()
{
wait1Msec(3);
int beginning = beginInterface();
wait1Msec(500);
int ending = interface();
wait1Msec(500);
int timing = timeinterface();
servo(servo3) = 0;
waitForStart();
wait10Msec(timing * 100);
initialize();
wait10Msec(30);
switch(beginning) // test 'nTaskToStart' in the switch
{
case 2:
// Move away from the wall
motor[MOTOR_LEFT] = -40;
motor[MOTOR_RIGHT] = -40;
driveTo(4);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the west wall
gyroTurn(20, 87);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
// Drive toward the west wall
motor[MOTOR_LEFT] = -40;
motor[MOTOR_RIGHT] = -40;
driveTo(25);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the baskets
gyroTurn(-20, 40);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
case 3:
//Move away from the wall
motor[MOTOR_LEFT] = -40;
motor[MOTOR_RIGHT] = -40;
driveTo(2);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
case 4:
case 1:
default:
}
//-------------------------------
// We're aligned with the baskets.
// Start walking along them looking for the IR beacon
motor[MOTOR_LEFT] = -60;
motor[MOTOR_RIGHT] = -63;
driveTo(4);
if(CheckIR(4) || checkIR(5)) // The first basket
{
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
wait10Msec(4);
servoTarget(servo3) = 180;
wait10Msec(50);
//forward/back
}else { // The second basket
driveTo(13);
if(checkIR(4) || checkIR(5))
{
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
servoTarget(servo3) = 180;
wait10Msec(150);
//forward/back
}else{ // The third basket
driveTo(35);
if(sensorValue[IRsensor] == 6 || sensorValue[IRsensor] == 5)
{
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
servoTarget(servo3) = 180;
wait10Msec(150);
//place block
//forward/back
}else{ // The fourth basket
driveTo(45);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
servoTarget(servo3) = 180; // Deploy the auto-scoring arm
wait10Msec(150);
//forward/back
}
}
}
servoTarget[servo3] = 0; // Retract the auto-scoring arm
switch(ending) // test 'nTaskToStart' in the switch
{
case 1: // if 'nTaskToStart' is '1':
// Drive to the end of the baskets.
motor[MOTOR_LEFT] = 100;
motor[MOTOR_RIGHT] = 97;
while(abs(nMotorEncoder[MOTOR_RIGHT]) > 2000)
{
}
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the ramp
gyroTurn(20, 65);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
// Drive in front of the ramp
motor[MOTOR_LEFT] = 90;
motor[MOTOR_RIGHT] = 90;
driveTo(20);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
// Turn toward the ramp agai
gyroTurn(20, 30);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = 90;
motor[MOTOR_RIGHT] = 90;
driveTo(25);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
gyroTurn(-20, 85);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -100;
motor[MOTOR_RIGHT] = -100;
driveTo(40);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
break; // break out of this switch statement and continue code after the '}'
case 2: // if 'nTaskToStart' is '2':
motor[MOTOR_LEFT] = -90;
motor[MOTOR_RIGHT] = -90;
driveTo(51);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -90;
motor[MOTOR_RIGHT] = -90;
driveTo(10);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
gyroTurn(-40, 70);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -50;
motor[MOTOR_RIGHT] = -50;
driveTo(35);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
gyroTurn(-40, 90);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = -90;
motor[MOTOR_RIGHT] = -90;
driveTo(35);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0;
/* gyroTurn(-20, 70);
nMotorEncoder[MOTOR_LEFT] = 0;
nMotorEncoder[MOTOR_RIGHT] = 0;
motor[MOTOR_LEFT] = 90;
motor[MOTOR_RIGHT] = 90;
driveTo(40);
motor[MOTOR_LEFT] = 0;
motor[MOTOR_RIGHT] = 0; */
// start task Two
break; // break out of this switch statement and continue code after the '}'
default: // if 'nTaskToStart' is anything other than '1' or '2':
// start task Three
}
}
void CBOT_main( void )
{
// initialize the robot
initializeRobot();
currentOrientation = NORTH;
// Ask for Goal
char isDone = 0;
unsigned char btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Goal?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
currentGoalWorld--;
currentGoalWorld = currentGoalWorld&0b1111;
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
currentGoalWorld++;
currentGoalWorld = currentGoalWorld&0b1111;
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Ask for starting orentation
isDone = 0;
btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Orient?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
// If we move left
// shift our oriention CCW
currentOrientation--;
currentOrientation = currentOrientation&0b11;
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
// If we move right
// shift our oriention CW
currentOrientation++;
currentOrientation = currentOrientation&0b11;
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Ask to start
isDone = 0;
btnHolder = UNPRESSED;
LCD_clear();
LCD_printf(" Start?\n\n\n\n");
while(!isDone){
btnHolder = EnterTopoCommand();
switch(btnHolder){
case MOVE_LEFT:
break;
case MOVE_FORWARD:
isDone = 1;
break;
case MOVE_RIGHT:
break;
default:
break;
}
printMap(currentOrientation,currentGoalWorld,RESET);
TMRSRVC_delay(100);//wait .1 seconds
}
// Locilize the Robot
// localize();
// Initialize State
isLost = 1;
oldMove = MOVE_STOP;
// Localization Loop
while(isLost)
{
// Break if not isLost
if(!isLost){
break;
}
//Sense Gateway
checkIR();
checkWorld();
//Plan using the Gateway
planGateway();
//Localize from Gateways?
isLost = localizeGateway();
//Act on the Gateway
moveMap();
}
// Update the currentOrientation using currentMove
switch(currentMove){
// case MOVE_LEFT:
// // If we move left
// // shift our oriention CCW
// currentOrientation--;
// currentOrientation = currentOrientation&0b11;
// break;
case MOVE_FORWARD:
break;
// case MOVE_RIGHT:
// // If we move right
// // shift our oriention CW
// currentOrientation++;
// currentOrientation = currentOrientation&0b11;
// break;
default:
LCD_printf("Whatz2?!");
break;
}
SPKR_beep(500);
// LCD_clear();
// LCD_printf("LOLZ\nI'm found!");
// TMRSRVC_delay(3000);//wait 3 seconds
LCD_clear();
LCD_printf(" New Map\n\n\n\n");
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(1000);//wait 1 seconds
SPKR_beep(0);
// currentCellWorld = 0;
isFire = 0;
// Go firefight
while(!isFire){
//Sense Gateway
checkIR();
checkWorld();
isFire = checkFire();
if(isFire){
break;
}
// Plan using Map
planMap();
// Shift the map
currentCellWorld = shiftMap(currentCellWorld, currentMove, currentOrientation);
// Act on the Map
moveMap();
}
// // Beep for the fire SIREN
// int ii;
// for (ii=0; ii<=3; ii++){
// SPKR_beep(250);
// TMRSRVC_delay(1000);
// SPKR_beep(500);
// TMRSRVC_delay(1000);
// }
// SPKR_beep(0);
// // Print the fire cell location
// LCD_clear();
// LCD_printf("Fire = %i\n\n\n\n", currentFireCell);
// TMRSRVC_delay(5000);
// Moves the Robot to the goal
metric();
// Print that you are at home and the fire cell location
LCD_clear();
LCD_printf("LOLZ\nI'm HOME\nFire at Cell: %i\n\n",currentFireCell);
// Stop when home is reached
STEPPER_stop(STEPPER_BOTH, STEPPER_BRK_OFF);
// Beep when home is reached
SPKR_beep(500);
TMRSRVC_delay(3000);//wait 3 seconds
SPKR_beep(0);
TMRSRVC_delay(7000);//wait 7 seconds
/**
// Enter the robot's current (starting) position
LCD_printf("START Map/nlocation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
worldInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot's current (starting) orientation
LCD_printf("START Map/norientation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
orientationInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Print the map
LCD_clear();
printMap(currentOrientation,currentCellWorld,RESET);
TMRSRVC_delay(10000);//wait 10 seconds
LCD_clear();
// Enter the robot's current (starting) position
LCD_printf("START Path\nlocation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
worldInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot's current (starting) orientation
LCD_printf("START Path\norientation\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
orientationInput();
TMRSRVC_delay(1000);//wait 3 seconds
LCD_clear();
// Enter the robot topological commands
LCD_printf("ENTER Path\ncommands\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
movesInput();
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
// Print the robot gateways
LCD_printf("Robot Gateways:\n\n\n\n");
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
getGateways();
TMRSRVC_delay(1000);//wait 1 seconds
LCD_clear();
// Goal loop
while (1)
{
checkIR();
checkWorld();
moveWorld();
//Test arc function
// LCD_printf("Move Arc\n\n\n\n");
// TMRSRVC_delay(1000);//wait 1 seconds
// move_arc_stwt(POINT_TURN, LEFT_TURN, 10, 10, 0);
// //Test contact Sensors
// LCD_printf("Right Contact: %i\nLeft Contact: %i\n\n\n",rightContact,leftContact);
// TMRSRVC_delay(1000);//wait 1 seconds
// // Test IR Sensors
// LCD_clear();
// LCD_printf("FrontIR = %3.2f\nBackIR = %3.2f\nLeftIR = %3.2f\nRightIR = %3.2f\n", ftIR,bkIR,ltIR,rtIR);
// TMRSRVC_delay(1000);//wait 1 seconds
}
**/
}// end the CBOT_main()
