theodore.c

#pragma config(Sensor, S1,     sonarSensor,         sensorSONAR)
#pragma config(Sensor, S2,     gpsSensor,           sensorLowSpeed)
#pragma config(Motor,  motorA,          right,         tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          sensorMotor,   tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          left,          tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/DGPS-driver.h"

task forwardTask();
task forkInTheRoadTask();
task obstacleDetectionTask();
task gpsCoordsTask();
void turnRight();
void turnLeft();
void allStop();
void backup();




task main() {
  // Setup for precision control of sensor motor
  nMotorEncoder[sensorMotor] = 0;
  nMotorEncoderTarget[sensorMotor] = 0;

  StartTaskWithPriority(forwardTask, kDefaultTaskPriority);
  StartTaskWithPriority(obstacleDetectionTask, kHighPriority);
  StartTaskWithPriority(gpsCoordsTask, kLowPriority);

  while (nNxtButtonPressed!=0) {
    wait1Msec(500);
  }
}

//***********************************************************************
/* Tasks */
//***********************************************************************
task forwardTask() {
  PlaySound(soundUpwardTones);
  int forwardSpeed = 10;
  int spins = 0;

  motor[left] = forwardSpeed;
	motor[right] = forwardSpeed;

  while (true) {
	  if (spins >= 20) {
	    spins = 0;
	    // Increase speed if we're not already at max
	    if (forwardSpeed < 100) {
	      forwardSpeed += 10;
	      motor[left] = forwardSpeed;
	      motor[right] = forwardSpeed;
	    }
	  }

	  spins++;
	  wait1Msec(250);
	}

	return;
}

task forkInTheRoadTask() {
  // First backup
  backup();

  // Look Right
  nMotorEncoderTarget[sensorMotor] = 90;
	motor[sensorMotor] = -25;
	wait1Msec(1000);
  int rightSonar = SensorValue(sonarSensor);
	nxtDisplayCenteredTextLine(0, "Right Sonar");
	nxtDisplayCenteredBigTextLine(2, "%d", rightSonar);
	wait1Msec(500);

	// Look Left
	nMotorEncoderTarget[sensorMotor] = 180;
	motor[sensorMotor] = 25;
	wait1Msec(2000);
	int leftSonar = SensorValue(sonarSensor);
	nxtDisplayCenteredTextLine(0, "Left Sonar");
	nxtDisplayCenteredBigTextLine(2, "%d", leftSonar);
	wait1Msec(500);

	// Back to Front
	nMotorEncoderTarget[sensorMotor] = 90;
	motor[sensorMotor] = -25;
	wait1Msec(1000);

	// Now decide which direction to go
	if (rightSonar > leftSonar) {
	  turnRight();
	} else {
	  turnLeft();
	}

  StartTaskWithPriority(forwardTask, kDefaultTaskPriority);
  StartTaskWithPriority(obstacleDetectionTask, kHighPriority);
  return;
}

task obstacleDetectionTask() {
  bool obstacleDetected = false;
  while (!obstacleDetected) {
    int sonarValue = SensorValue(sonarSensor);
		nxtDisplayCenteredTextLine(0, "Front Sonar");
		nxtDisplayCenteredBigTextLine(2, "%d", sonarValue);

    if(sonarValue < 30) {
      // Shit, there's something in the way
      StopTask(forwardTask);
      allStop();
      PlaySound(soundException);
      obstacleDetected = true;
    } else {
     wait1Msec(10);
    }
  }
  StartTask(forkInTheRoadTask);
  return;
}


task gpsCoordsTask() {
  while(true) {
    float lat = (float)DGPSreadLatitude(S2)/1000000;     // Lat will be in decimal degrees
    float lng = (float)DGPSreadLongitude(S2)/1000000;      // Lon will be in decimal degrees

    nxtDisplayCenteredTextLine(4, "Lat: %3.6f", lat);
    nxtDisplayCenteredTextLine(5, "Lng: %3.6f", lng);
    wait1Msec(3000);
  }
}
//***********************************************************************

//***********************************************************************
/* Functions */
//***********************************************************************

void allStop() {
  motor[left] = 0;
  motor[right] = 0;
  return;
}

void backup() {
  motor[left] = -30;
  motor[right] = -30;
  wait1Msec(1000);
  motor[left] = 0;
  motor[right] = 0;
  return;
}

void turnRight() {
  // To turn right, we need to go forward on the left motor,
  // and go back on the right motor;
  motor[left] = 30;
  motor[right] = -30;
  wait1Msec(500);
  allStop();
  return;
}

void turnLeft() {
  // To turn left, we need to go forward on the right motor,
  // and go back on the left motor;
  motor[right] = 30;
  motor[left] = -30;
  wait1Msec(500);
  allStop();
  return;
}