task main()
{
pointTurn(left); // make a sharp left turn
wait(1.5); // wait for 1.5 seconds
stop(); // stop
wait(1.0); // wait for 1.0 seconds
pointTurn(right); // make a sharp right turn
wait(1.5); // wait for 1.5 seconds
stop(); // stop
}
task main()
{
forward(50); // drive forward at speed 50
untilTouch(); // continue until the touch sensor is pressed
backward(50); // drive backward at speed 50
wait(1.5); // wait 1.5 seconds
pointTurn(right); // make a sharp right turn
wait(0.75); // wait 0.75 seconds
stop(); // stop the robot
}
void turnaround(double distance, char opt) {
goCM(distance); //go forward that distance
if (opt == 'p') { //do a point turn
pointTurn(180);
}
else if (opt == 'u') {
circle0(0, 180); //do a half circle for a U-turn
}
else if (opt == 't') {
circle0(THREE_TURN_RADIUS, 60); //first half-sort-of-turn for the three-point turn
reverseCircle0(THREE_TURN_RADIUS, 60);
circle0(THREE_TURN_RADIUS, 65);
}
goCM(distance); //returns back to its original place
}
void driveStraight(float dist)
{
float timeStart = get_time();
Point prevPt = {currentPt.x, currentPt.y};
motor_vel = speedLimit(KD*dist);
straight(motor_vel);
pause(500);
update();
if (distanceTo(prevPt) < 0.01) //working too hard, what to do?
{
brake();
wiggle(5);
straight(70); //backward at speed 70
pause(1000);
pointTurn(0);
}
}
void goToPoint()
{
update();
printf("VPS Current Point: (%d, %d)\n", game.coords[0].x, game.coords[0].y);
if(currentPt.y > 0){
desiredPt.x = 3.3;
desiredPt.y = 2.5;
}
else{
desiredPt.x = 3.3;
desiredPt.y = -2.6;
}
printf("Current Point: (%f, %f)\n", currentPt.x, currentPt.y);
printf("Desired Point: (%f, %f)\n", desiredPt.x, desiredPt.y);
printf("Our current heading is: %f\n", (float)game.coords[0].theta/2047*180); //NEED TO CONVERT TO DEGREES
float targetHeading = getTargetTheta(desiredPt);
printf("The target heading is: %f\n", targetHeading);
pointTurn(targetHeading);
float dist = distanceTo(desiredPt);
printf("The distance is: %f\n", dist);
while(dist > TOLERANCE)
{
driveStraight(dist);
printf("Trying to drive straight\n");
update();
dist = distanceTo(desiredPt);
}
brake();
//get location data
//determine angle and position relative to desired point
//turn to face desired point
//drive "straight" to point (unless obstacles)
}
void square(int distance) {
for (int i = 0; i < 4; i++) { //to make a square, we go forward and turn 4 times
goCM(distance); //go a side length
pointTurn(90); //do a 90 turn
}
}
