Untitled
unknown
plain_text
3 years ago
14 kB
2
Indexable
Never
#if 1//Maze
void tank_turn_l (uint8 l_speed, uint8 r_speed, uint32 delay)
{
MotorDirLeft_Write(1);
MotorDirRight_Write(0);
PWM_WriteCompare1(l_speed);
PWM_WriteCompare2(r_speed);
vTaskDelay(delay);
}
void tank_turn_r (uint8 l_speed, uint8 r_speed, uint32 delay)
{
MotorDirLeft_Write(0);
MotorDirRight_Write(1);
PWM_WriteCompare1(l_speed);
PWM_WriteCompare2(r_speed);
vTaskDelay(delay);
}
void prepare (void)
{
int line, line2;
struct sensors_ ref, dig;
IR_Start();
IR_flush();
reflectance_start();
reflectance_set_threshold(9000, 9000, 11000, 11000, 9000, 9000); // set center sensor threshold to 11000 and others to 9000
// When button is pressed, go to the next line and wait there for an IR command to start main program.
for(;;)
{
while (SW1_Read() == 0)
{
for(;;)
{
motor_forward(100,100);
line2 = dig.r3 + dig.l3;
reflectance_read(&ref);
reflectance_digital(&dig);
line = dig.r3 + dig.l3;
if( line2 - line == 2)
{
motor_forward(100,50);
motor_forward(0,0);
IR_wait();
motor_forward(100,50);
return ;
}
}
}
}
}
void zmain(void)
{
motor_start();
motor_forward(0,0);
struct sensors_ dig;
reflectance_start();
reflectance_set_threshold(9000, 9000, 11000, 11000, 9000, 9000); // set center sensor threshold to 11000 and others to 9000
IR_Start();
IR_flush();
Ultra_Start();
int distance, distance2 , x = 0, z = 0, y = 0, u = 0, end_sequence = 0;
TickType_t start, end ,runtime;
send_mqtt("Zumo016/debug", "Preparations are ready, Sir! Standby for launch!");
prepare();
start = xTaskGetTickCount() /1000;
print_mqtt("Zumo016/debug", "Start time: %ds.", start);
for(;;)
{
reflectance_digital(&dig);
distance = Ultra_GetDistance();
//If object is seen, activate turn left (x variable) on next black line
if (distance < 25 && y == 0) // Y is used to take the measurement only once
{ //resets when avoid 1 breaks
Beep(100,100);
print_mqtt("Zumo016/debug","Uh-oh, I need to DOOODGE!");
x = 1; //If obstacle is seen, activate avoid state 1(x)
y = 1;
}
// If all sensors hit white, turn left, (drive state)
if (dig.l3 ==0 && dig.l2 ==0 && dig.l1 ==0 && dig.r1 ==0 && dig.r2 ==0 && dig.r3 ==0)
{
tank_turn_l(200,200,220);
motor_forward(100,50);
end_sequence++; // Program is shut down when end_sequence reaches 2,
} //in other words when it turns left twice in a row.
else
{
end_sequence = 0;
}
// If all sensors hit black , turn right (drive state)
if (dig.l3 ==1 && dig.l2 ==1 && dig.l1 ==1 && dig.r1 ==1 && dig.r2 ==1 && dig.r3 ==1)
{
motor_forward(100,100);
tank_turn_r(200,200,210);
motor_forward(100,50);
}
// Drive state line following
else if( dig.l1 ==1 && dig.r1 ==1 && dig.r2 == 0)
{
motor_forward(140,30);
}
else if(dig.r3 ==1 && dig.r2 ==1 && dig.r1 ==0 )
{
motor_turn(150,0,50);
}
else if(dig.l2 ==1 && dig.l1 ==1 && dig.r1 ==0)
{
motor_turn(20,120,50);
}
else if(dig.l1 ==0 && dig.r1 ==1 && dig.r2 ==1)
{
motor_turn(120,20,30);
}
if( dig.l3 ==1 && dig.l2 ==1 && dig.r2 ==0) //Enter avoid state if obstacle was seen.
{
if (x == 1)
{
motor_forward(100,50);
tank_turn_l(200,200,210);
motor_forward(100,50);
do //Avoid state
{
reflectance_digital(&dig);
// Let the games begin.
// Moves forwards until black line is seen, then turns right to check if path is clear
if(dig.l3 ==1 && dig.l2 ==1 && dig.l1 ==1 && dig.r1 ==1 && dig.r2 ==1 && dig.r3 ==1)
{
if (u == 1) //Activator for "dodge lvl 2"
{
motor_forward(100,100);
tank_turn_l(200,200,210);
motor_forward(100,50);
u = 2;
continue;
}
// Turn right to check the path, "avoid lvl 1"
// Avoid lvl 1 means dodging obstacle vertically compared
// to the direction start-finish
motor_forward(100,100);
tank_turn_r(200,200,230);
motor_forward(0,0);
vTaskDelay(50);
distance = Ultra_GetDistance();
if(distance > 35)
{
if (u == 2) // If avoid lvl 2 was activated, repeat avoid lvl 1
{ // lvl 2 avoid is dodging obstacle in horizontal axis
// during lvl 2 avoid the robot comes towards the starting line.
motor_forward(100,50);
u = 0;
continue;
}
motor_forward(100,50); // To keep the bot on the line
for(;;)
{
// Activate avoid lvl 1 again if obstacle is seen after dodging (if statement below)
reflectance_digital(&dig);
distance = Ultra_GetDistance();
if (distance < 25)
{
x = 1;
if (dig.r1 ==1 && dig.r2 ==1 && dig.l2 ==1 && dig.l1 == 1)
{
Beep(100,100);
motor_forward(75,50);
tank_turn_l(200,200,210);
motor_forward(100,50);
break;
}
}
//After a clear path is discovered after dodge lvl 1, move x lines forward
// and turn right
if (dig.l1 ==1 && dig.r1 ==1) //Line checking
{
if(dig.r2 ==1 && dig.r3 ==1)
{
x++; // counter for which line to turn right on
if(x == 6)
{
motor_forward(100,100);
tank_turn_r(200,200,230);
}
motor_forward(100,50); // Minor adjustment to keep the bot on line
}
else
{
motor_forward(170,50);
}
}
//These are line following rules after a clear path was found
//during dodge lvl 1.
else if( dig.l2 == 1 && dig.l1 ==1 && dig.r1 == 0)
{
motor_turn(20,120,30);
}
else if( (dig.l3 == 1 && dig.l2 ==1 && dig.l1 == 0) || (dig.l3 == 1 && dig.l2 ==0 && dig.l1 == 0))
{
motor_turn(0,130,30);
}
else if(dig.l1 ==0 && dig.r1 ==1 && dig.r2 == 1)
{
motor_turn(120,20,30);
}
else if((dig.r1 ==0 && dig.r2 ==1 && dig.r3 == 1) || (dig.r3 == 1 && dig.r2 ==0 && dig.r1 == 0))
{
motor_turn(130,0,30);
}
if(dig.l3 ==0 && dig.l2 ==0 && dig.l1 ==0 && dig.r1 ==0 && dig.r2 ==0 && dig.r3 ==0)
{
motor_backward(100,65); // Break into drive state on white line,
break;
}
}
}
else // If there was no clear path, turn back left and try again
{
print_mqtt("Zumo016/debug","Can't go through here!");
tank_turn_l(200,200,220);
motor_forward(100,100);
}
}
distance2 = Ultra_GetDistance();
if(distance2 < 25 && z == 0) // If obstacle is seen DURING 1st lvl dodge, activate lvl 2 dodge
{
print_mqtt("Zumo016/debug","Did the designer do this on purpose?");
Beep (100,100);
z = 1; // lvl 1 avoid state activation.
u = 1;
} // basic line following again
if (dig.l1 ==1 && dig.r1 ==1) // used during lvl 1 avoiding
{
motor_forward(130,30);
}
else if( dig.l2 == 1 && dig.l1 ==1 && dig.r1 == 0)
{
motor_turn(20,120,30);
}
else if( (dig.l3 == 1 && dig.l2 ==1 && dig.l1 == 0) || (dig.l3 == 1 && dig.l2 ==0 && dig.l1 == 0))
{
motor_turn(0,150,30);
}
else if(dig.l1 ==0 && dig.r1 ==1 && dig.r2 == 1)
{
motor_turn(120,20,30);
}
else if((dig.r1 ==0 && dig.r2 ==1 && dig.r3 == 1) || (dig.r3 == 1 && dig.r2 ==0 && dig.r1 == 0))
{
motor_turn(150,0,30);
}
// Breaks the dodge loop, return to drive status when bot has reached the right hand side
}while(dig.l3 ==1 || dig.l2 ==1 || dig.l1 ==1 || dig.r1 ==1 || dig.r2 ==1 || dig.r3 ==1);
motor_forward(100,100);
tank_turn_l(200,200,220);
motor_forward(100,100);
//set variables to enable avoid lvl 1 again
x = 0;
y = 0;
}
else if(dig.l3 ==1 && dig.l2 ==1 && dig.l1 ==0 ) //Minor adjustment after reaching "white"
{
motor_turn(0,150,50);
}
else
{
motor_forward(100,20); //if x = 0, just move forward
} // else option for avoid state 1
}
if (SW1_Read() == 0 || (end_sequence == 2)) //End sequence when button is pressed
{
motor_forward(0,0);
end = xTaskGetTickCount() /1000;
print_mqtt("Zumo016/debug","Stop time %ds.", end);
runtime = end - start;
print_mqtt("Zumo016/debug","We were adventuring for %ds.", runtime);
for(;;)
{
vTaskDelay(200);
}
}
}
}
#endif