// funcion donde se ejecuta la logica, se llama cada vez que se cumple el tiempo configurado en timer
void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)
{
if (!g_pose) // si g_pose==0, termina la funcion
{
return;
}
if (!g_first_goal_set) // si no hubiese una meta, ejecuta:
{
g_first_goal_set = true;
g_state = FORWARD;
g_goal.x = distance[d]*cos(g_pose->theta) + g_pose->x;
g_goal.y = distance[d]*sin(g_pose->theta) + g_pose->y;
g_goal.theta = g_pose->theta;
d++;
printGoal();
}
if (g_state == FORWARD) // si el estado es FORWARD, ejecuta:
{
forward(twist_pub); // envia la orden de movimiento a la funcion forward
}
else if (g_state == STOP_FORWARD) // si el estado es STOP_FORWARD, ejecuta:
{
stopForward(twist_pub); // envia la orden de detenerse a la funcion stop_forward
}
else if (g_state == TURN) // si el estado es TURN, ejecuta:
{
turn(twist_pub); // envia la orden de giro a la funcion turn
}
else if (g_state == STOP_TURN) // si el estado es STOP_TURN, ejecuta:
{
stopTurn(twist_pub); // envia la orden de detener giro a la funcion stop_turn
}
}
void timerCallback(const ros::TimerEvent&, ros::Publisher vel_pub)
{
if (!g_pose)
{
return;
}
if (!g_first_goal_set)
{
g_first_goal_set = true;
g_state = FORWARD;
g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
g_goal.theta = g_pose->theta;
printGoal();
}
if (g_state == FORWARD)
{
forward(vel_pub);
}
else if (g_state == STOP_FORWARD)
{
stopForward(vel_pub);
}
else if (g_state == TURN)
{
turn(vel_pub);
}
else if (g_state == STOP_TURN)
{
stopTurn(vel_pub);
}
}
task main()
{
while(SensorValue[bumperLeft] == 0)
{
}
StartTask(intakeStart);
//StartTask(armcontrol);
//armPos = 1000;
// Start from hanging zone (Top left)
ClearTimer(T4);
moveSecondTierUp(127,450);
moveSecondTierDown(127,0);
intake = 1;
motor[secondTier]=-127;
wait10Msec(50);
motor[secondTier]=0;
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(50);
turnRight(127,600);
moveFirstTierUp(127,300);
motor[firstTierLeft]=20;
motor[firstTierRight]=20;
intake = 0;
moveStraightTime(127, 2000);
moveFirstTierDown(127, 0);
// Continue from middle zone (Left)
while (SensorValue[bumperLeft] == 0)
{
}
moveStraightDistance(127,350);
alignFoward(127);
stopPid(0.6,0.3);
moveStraightDistance(100,50);
stopPid(0.6,0.3);
wait10Msec(10);
turnRight(100,280);
stopTurn(0.6,0.3);
moveStraightDistance(100,1400);
stopPid(0.6,0.3);
moveFirstTierUp(127,1200);
motor[firstTierLeft] = 10;
motor[firstTierRight] = 10;
moveStraightDistance(100,200);
turnLeft(50,30);
stopTurn(0.6,0.3);
moveStraightTime(50,500);
intake = -1;
wait10Msec(300);
intake = 0;
moveStraightDistance(-50,250);
moveFirstTierDown(127,50);
turnLeft(127,40);
moveStraightDistance(-100, 1300);
// Continue from Middle Zone (Left)
while (SensorValue[bumperLeft] == 0)
{
}
moveStraightDistance(127, 1500);
alignFoward(127);
intake = 1;
moveStraightDistance(40, 500);
wait10Msec(50);
intake = 0;
// Continue from Middle Zone (Right)
while (SensorValue[bumperLeft] == 0)
{
}
moveStraightDistance(-127,300);
moveFirstTierUp(127,400);
motor[firstTierLeft]=20;
motor[firstTierRight]=20;
moveStraightDistance(127,1800);
moveFirstTierDown(127,400);
// Continue from Hanging Zone (Top right)
while (SensorValue[bumperLeft] == 0)
{
}
intake = 1;
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(170);
intake = 0;
turnLeft(127,650);
intake = 1;
moveFirstTierUp(127,400);
motor[firstTierLeft]=10;
motor[firstTierRight]=10;
intake = 0;
moveStraightDistance(127, 1300);
moveFirstTierUp(127, 500);
motor[firstTierLeft]=10;
motor[firstTierRight]=10;
// Continue from Middle Zone (Right)
while (SensorValue[bumperLeft] == 0)
{
}
moveStraightDistance(127,350);
stopPid(0.6,0.3);
alignFoward(127);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
wait10Msec(10);
turnLeft(127,380);
moveStraightDistance(127,1300);
stopPid(0.6,0.3);
moveFirstTierUp(127,1100);
moveStraightDistance(100,100);
motor[firstTierLeft] = 10;
motor[firstTierRight] = 10;
turnRight(50,30);
moveStraightDistance(50,200);
intake = -1;
wait10Msec(300);
intake = 0;
moveStraightDistance(-50,250);
moveFirstTierDown(127,50);
turnRight(127,50);
moveStraightDistance(-50, 1300);
// Continue from Middle Zone (Right)
while (SensorValue[bumperLeft] == 0)
{
}
moveFirstTierUp(127,1100);
motor[firstTierLeft] = 20;
motor[firstTierRight] = 20;
moveStraightDistance(127,800);
moveStraightDistance(-127,600);
// Continue from Middle Zone (Right)
while (SensorValue[bumperLeft] == 0)
{
}
moveStraightDistance(127,1200);
moveStraightDistance(-127,500);
turnRight(127,350);
moveStraightDistance(127,1500);
// Continue from Middle Zone (Left)
while (SensorValue[bumperLeft] == 0)
{
}
moveStraightDistance(127,800);
moveStraightDistance(-127,600);
// Continue from Middle Zone (Left)
while (SensorValue[bumperLeft] == 0)
{
}
moveStraightDistance(127,1000);
moveStraightDistance(-127,1100);
}
