#pragma config(Sensor, S1,     touchSensor,    sensorTouch)
#pragma config(Sensor, S2,     compassSensor,  sensorVirtualCompass)
#pragma config(Sensor, S3,     lightSensor,    sensorLightActive)
#pragma config(Sensor, S4,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  motorA,          gripperMotor,  tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*+++++++++++++++++++++++++++++++++++++++++++++| Notes |++++++++++++++++++++++++++++++++++++++++++++++
H2 Lift Road - Fueling the Rocket (NXT Standard ROBOTC)
- This program will cause the robot to drive forward from Sector 1 to the Ion Propulsion Fuel Barrel
near map intersection G3, pick it up, and turn toward the F3 Intraplanetary Boost Pad.
- It will be up to you to add the ROBOTC code that causes the robot to drive onto the Boost Pad,
further into the zone, and then release the Fuel Barrel onto the Fuel Station.
- Make sure that the GRIPPER attachment is selected on the bottom of the Operation Reset window.
- Remember that you can press the "Reset" button in the lower-left corner of the Operation Reset
window to return to the Insertion Point every time you modify your ROBOTC code.
*   Please note that the values in this program were written for Mammalbot using a high-end computer.
You may need to adjust the values in this program for your computer.

Robot Model: Mammalbot

[I/O Port]          [Name]              [Type]                   [Description]
Motor Port A        gripperMotor        Motor w/ Encoder           Front gripper motor
Motor Port B        rightMotor          Motor w/ Encoder           Right side motor
Motor Port C        leftMotor           Motor w/ Encoder           Left side motor
----------------------------------------------------------------------------------------------------*/

//Every program has a task main(). All commands that the robot follows go between the curly braces.
task main()
{   //The program begins.
   wait1Msec(2000);      //The robot will wait for 2 seconds before executing commands.

   //Reset the encoders to 0.
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;

   while(nMotorEncoder[rightMotor] < 1580)   //while less than 1580 counts of the right motor encoder have passed...
   {
      //...move forward.
      motor[rightMotor] = 50;
      motor[leftMotor] = 50;
   }

   //Stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;

   wait1Msec(1000);      //The robot will wait for 1 second before executing other commands.

   //Reset the encoders to 0.
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;

   while(nMotorEncoder[leftMotor] < 50)   //while less than 50 counts of the left motor encoder have passed...
   {
      //...turn right.
      motor[rightMotor] = -50;
      motor[leftMotor] = 50;
   }

   //Stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;

   wait1Msec(1000);      //The robot will wait for 1 second before executing other commands.

   motor[gripperMotor] = 40;         //Turn the gripperMotor on POSITIVE to CLOSE it around the Fuel Barrel.
   wait1Msec(1000);                     //Give the gripperMotor 1 second to close...
   motor[gripperMotor] = 0;         //...before stopping the gripperMotor.

   wait1Msec(1000);      //The robot will wait for 1 second before executing other commands.

   //Reset the encoders to 0.
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;

   while(nMotorEncoder[leftMotor] < 60)   //while less than 60 counts of the left motor encoder have passed...
   {
      //...turn right.
      motor[rightMotor] = -50;
      motor[leftMotor] = 50;
   }

   //Stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;

   //Enter the code for the robot to drive forward to the Interplanetary Boost Pad here:


   //Enter the code for the robot to drive forward to the Fuel Station here:


   //Enter the code for the robot to drop off the Fuel Barrel here:
   //Note: NEGATIVE motor power levels will OPEN the Gripper and release the Fuel Barrel!


}   //The program ends.

#pragma config(Sensor, S1,     touchSensor,    sensorTouch)
#pragma config(Sensor, S2,     compassSensor,  sensorVirtualCompass)
#pragma config(Sensor, S3,     lineFollower,   sensorLightActive)
#pragma config(Sensor, S4,     sonarSensor,    sensorSONAR)
#pragma config(Motor,  motorA,          vacuumMotor,   tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          rightMotor,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          leftMotor,     tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*+++++++++++++++++++++++++++++++++++++++++++++| Notes |++++++++++++++++++++++++++++++++++++++++++++++
Crystal Path Insertion Point (NXT Standard ROBOTC)
- This program will cause the robot to track the line from Sector 1 to the Unobtanium Cystal
along Cystal Path, and activate the Vacuum attachment to absorb the Crystal.
- It will be up to you to deliver the Unobtanium Cystal by stopping the robot on the Cystal pad
near the Rocket on the upper level.
- Make sure that the VACUUM attachment is selected on the bottom of the Operation Reset window.
- Remember that you can press the "Reset" button in the lower-left corner of the Operation Reset
window to return to the Insertion Point every time you modify your ROBOTC code.
*   Please note that the values in this program were written for Mammalbot using a high-end computer.
You may need to adjust the values in this program for your computer.

Robot Model: Mammalbot

[I/O Port]          [Name]              [Type]                   [Description]
Motor Port A        vacuumMotor           Motor w/ Encoder           Front vacuum motor
Motor Port B        rightMotor          Motor w/ Encoder           Right side motor
Motor Port C        leftMotor           Motor w/ Encoder           Left side motor
Sensor Port 3            lightSensor               Line Following Sensor       Sensor facing bottom center
----------------------------------------------------------------------------------------------------*/

//Every program has a task main(). All commands that the robot follows go between the curly braces.
task main()
{   //The program begins.
   wait1Msec(2000);      //The robot will wait for 2 seconds before executing commands.

   //threshold is an integer variable that stores a value that is in between the white and black...
   //...line following values. It allows the robot to make decisions to track the line.
   int threshold = 25;

   //While less than 3240 counts of the right encoder have elapsed...
   while(nMotorEncoder[rightMotor] < 3240)
   {
      //...track the line.
      if(SensorValue[lineFollower] > threshold) //If the sensor sees white...
      {
         //...counter-steer right.
         motor[leftMotor]  = 50;
         motor[rightMotor] = 0;
      }
      else //Else, the sensor sees dark...
      {
         //...counter-steer left.
         motor[leftMotor]  = 0;
         motor[rightMotor] = 50;
      }
   }

   //Stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;

   wait1Msec(1000);      //The robot will wait for 1 second before executing other commands.

   //Reset the encoders to 0.
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;

   while(nMotorEncoder[leftMotor] < 140)   //while less than 140 counts of the left motor encoder have passed...
   {
      //...turn right.
      motor[rightMotor] = -50;
      motor[leftMotor] = 50;
   }

   //Stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;

   wait1Msec(1000);      //The robot will wait for 1 second before executing other commands.

   //Reset the encoders to 0.
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoder[leftMotor] = 0;

   while(nMotorEncoder[rightMotor] < 270)   //while less than 270 counts of the right motor encoder have passed...
   {
      //...move forward.
      motor[rightMotor] = 50;
      motor[leftMotor] = 50;
   }

   //Stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;

   wait1Msec(1000);      //The robot will wait for 1 second before executing other commands.

   motor[vacuumMotor] = -50;      //The vacuummMotor is activated.
   wait1Msec(1000);                  //Keep the vacuumMotor powered for 1 second.
   motor[vacuumMotor] = 0;         //The crystal is absorbed, turn the vacuumMotor off.

}   //The program ends.