// 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); }