void _start() {
while(1) {
switch(mode) {
case(FIND_MODE):
/* Anda em linha reta até encontrar parede */
set_motors_speed(10, 10);
if(find_wall() == 1) {
/* Encontrou uma parede, muda o modo
e pára o robo*/
mode = SET_POSITION_MODE;
set_motors_speed(0, 0);
}
break;
case(SET_POSITION_MODE):
/* Encontrou uma parede, posicione o lado esquerdo ao lado dela */
/* e mude o modo */
if(set_position_to_left() == 1) {
mode = FOLLOW_MODE;
}
break;
case(FOLLOW_MODE):
/* Siga a parede que foi encontrada */
follow_wall();
break;
}
}
}
task main()
{
/*initialise encoders*/
nMotorEncoder[motorA] = 0;
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
turnOut();
follow_wall();
//chamberAdjust();
/*
turnOut();
follow_wall();
//chamberAdjust();
turnOut();
follow_wall();
//chamberAdjust();
*/
}
task main()
{
writeDebugStream("-----------started big bothar f****r----------\n");
state_init();
bFloatDuringInactiveMotorPWM = false;
int cub = escape_cubicle();
if (cub == 1) {
// 1 -> 2 -> 3 -> 1
writeDebugStream("I just left cubicle 1\n");
// Rotate 90 to right
droid_rotate(90);
// Fine move the sonar to the left
sonar_move_at_async(90,true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 1 to 2
follow_wall(21, 1, false);
enter_cubicle(1);
// exit cube
droid_exit_cube();
droid_rotate(-90);
// Fine move the sonar to the left
sonar_move_at_async(90, true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 2 to 3
follow_wall(21, 1, false);
enter_cubicle(1);
// exit cube
droid_exit_cube();
droid_rotate(90);
// Fine move the sonar to the right
sonar_move_at_async(-90,true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 3 to 1
follow_wall(21, -1, true);
enter_cubicle(-1);
// We're done!
} else if (cub == 2) {
writeDebugStream("I just left cubicle 2\n");
// 2 -> 1 -> 3 -> 2
// Rotate 90 to right
droid_rotate(-90);
// Fine move the sonar to the right
sonar_move_at_async(-90,true)
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 2 to 1
follow_wall(21,-1, false);
enter_cubicle(-1);
// exit cube
droid_exit_cube();
droid_rotate(-90);
// Fine move the sonar back to left
sonar_move_at_async(90,true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 1 to 3
follow_wall(21,1,true);
enter_cubicle(1);
// exit cube
droid_exit_cube();
droid_rotate(90);
// Fine move the sonar back to left
sonar_move_at_async(-90,true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 3 to 2
follow_wall(21, -1, false);
enter_cubicle(-1);
// We're done!
} else if (cub == 3) {
// 3 -> 2 -> 1 -> 3
// Rotate 90 to right
droid_rotate(-90);
// Fine move the sonar to the right
sonar_move_at_async(-90,true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 3 to 2
follow_wall(21,-1, false);
enter_cubicle(-1);
// exit cube
droid_exit_cube();
droid_rotate(90);
// Fine move the sonar to the right
sonar_move_at_async(-90,true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 2 to 1
follow_wall(21,-1,false);
enter_cubicle(-1);
// exit cube
droid_exit_cube();
droid_rotate(-90);
// Fine move the sonar to the right
sonar_move_at_async(90,true);
// get to wall follow range.
droid_move(DEFAULT_POWER);
wait1Msec(1500);
// Wall follow from 1 to 3
follow_wall(21, 1, true);
enter_cubicle(1);
// We're done!
} else {
// better not get here
writeDebugStream("UNREACHABLE CODE REACHED\n");
StopAllTasks();
}
}
int main(void)
{
cli();
set_lcd();
set_motors();
set_ADC();
sei();
int getValue1(void); //This function returns the value of the sensor mounted on the arm
void findagte(void);
void entergate(void);
int follow_wall(void);
int getValue2(void) ;
int row;
int state;
centre = ADC_Conversion(2);
sensorLeft = ADC_Conversion(3);
sensorRight = ADC_Conversion(1);
value_front = ADC_Conversion(11);
value_4sens= ADC_Conversion(12);
value_2sens= ADC_Conversion(10);
value_1sens=ADC_Conversion(9);
/*
lcd_print(2, 1, sensorLeft, 3);
lcd_print(2, 5, centre, 3);
lcd_print(2, 9, sensorRight, 3);
*/
if(sensorLeft <50 && sensorRight <50 && centre < 50) //when the bot enters the arena first
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward_mm(350);
stop();
_delay_ms(100);
}
//getValue2();
value_front = ADC_Conversion(11);
if(value_front>150)
{
centre = ADC_Conversion(2);
while(centre<150)
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
value_front = ADC_Conversion(11);
if(value_front<150)
{
findgate();
}
centre = ADC_Conversion(2);
}
entergate();
}
else if(value_front < 50)
{
left_degrees(90);
stop();
_delay_ms(500);
//turn the gripper
servo_2(90);
while(1)
{
row=follow_wall();
if(state)
{
soft_right();
centre = ADC_Conversion(2);
while(centre<150)
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
if(value_front<150)
{
findgate();
}
centre = ADC_Conversion(2);
}
entergate();
}
}
}
return(row);
}
