#pragma config(Sensor, in15,    leftEncoder,               sensorQuadEncoder, int5)
#pragma config(Sensor, in16,    rightEncoder,            sensorQuadEncoder, int6)
#pragma config(Motor,    port1,                  leftMotor,                  tmotorNormal)
#pragma config(Motor,    port2,                  rightMotor,                  tmotorNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard                      !!*//

/*****************************************************************************/
/* Title:    Useful Robot Movement Functions for Skid Steer Robots                   */
/* Description:    A collection of functions designed to provide useful             */
/* locomotion to a robot that uses skid steering to drive.                            */
/* All length measurements are in centimeters and speed in -127 to 127          */
/* For proper use, motors and encoders should be named as follows:                */
/* leftMotor, rightMotor, leftEncoder, rightEncoder                                        */
/*****************************************************************************/

// Outside diameter of driven wheel (in cm), used to determine circumfrence.
int WheelDiameter = 5;
// Outside distance between driven wheels (in cm), used to determine turning circle.
int WheelBase = 10;
// Useful when encoder is not hooked directly to driven wheel.
int GearRatio = 1;
// Speed fuzziness, used to keep us going straight (will be doubled in use).
int SpeedFuzz = 1;
// Sleep time, used for turning and going straight.
int SleepTime = 500;

int DistancePerTick = (3.14 * WheelDiameter)/90 * GearRatio;
int TurningCircle = (3.14 * WheelBase);

void DriveStraight();
void DriveStraightDistance (int distance, int speed);
void DriveStraightTime (int time, int speed);
void PointTurnDegrees (int degree, int direction, int speed);
void WideTurnDegrees (int degree, int direction, int speed);

task main()
{
   // Go 10 cm at 100 speed.
   DriveStraightDistance(10,100);
   // Go 2 seconds at 100 speed.
   DriveStraightTime(2,100);
   // Pivot 90 degrees right (cw) at 100 speed.
   PointTurnDegrees(90, 1, 100);
   // Wide turn 90 degrees left (ccw) at 100 speed.
   WideTurnDegrees(90, -1, 100);
}

void DriveStraight()
{
   // Left side going faster than right.
   if (SensorValue[leftEncoder] > SensorValue[rightEncoder])
   {
      motor[leftMotor] -= SpeedFuzz;
      motor[rightMotor] += SpeedFuzz;
   }
   // Left side going slower than right.
   if (SensorValue[leftEncoder] < SensorValue[rightEncoder])
   {
      motor[leftMotor] += SpeedFuzz;
      motor[rightMotor] -= SpeedFuzz;
   }
}

// Drive straight given distance (in cm) at speed (-127 to 127).
void DriveStraightDistance (int distance, int speed)
{
   // Zero out variables and sensors
   int traveled = 0;
   SensorValue[leftEncoder] = 0;
   SensorValue[rightEncoder] = 0;

   // Lets get moving!
   motor[leftMotor] = speed;
   motor[rightMotor] = speed;

   while(traveled <= distance)
   {
      DriveStraight();
      wait1Msec(SleepTime);
      // Average the distance traveled by adding both sensors and dividing by 2.
      traveled = (SensorValue[leftEncoder] + SensorValue[rightEncoder])/2 * DistancePerTick;
   }
}

// Drive straight for time (in 1/100ths of a second at speed (-127 to 127).
void DriveStraightTime (int time, int speed)
{
   // Zero out variables, sensors and timers
   ClearTimer(T1);
   SensorValue[leftEncoder] = 0;
   SensorValue[rightEncoder] = 0;

   // Lets get moving!
   motor[leftMotor] = speed;
   motor[rightMotor] = speed;

   while(time100[T1] <= time)
   {
      DriveStraight();
      wait1Msec(SleepTime);
   }
}

// Point turn of degrees in direction at speed.
// -1 is left (ccw), 1 is right (cw)
// speed should be 0 to 127
void PointTurnDegrees (int degree, int direction, int speed)
{
   // Zero out variables and sensors
   int turned = 0;
   int traveled = 0;
   SensorValue[leftEncoder] = 0;
   SensorValue[rightEncoder] = 0;

   // Lets get moving!
   motor[leftMotor] = speed * direction;
   motor[rightMotor] = speed * direction * -1;

   while (turned <= degree)
   {
      DriveStraight();
      traveled = (SensorValue[leftEncoder] + SensorValue[rightEncoder]) * DistancePerTick;
      turned = (traveled / TurningCircle);
      wait1Msec(SleepTime);
   }
}

// Wide turn of degrees in direction at speed.
// -1 is left, 1 is right
// speed should be 0 to 127
void WideTurnDegrees (int degree, int direction, int speed)
{
   // Zero out variables and sensors
   int turned = 0;
   int traveled = 0;
   SensorValue[leftEncoder] = 0;
   SensorValue[rightEncoder] = 0;

   // Turning left, stop left motor and power up right.
   if (direction == -1)
   {
      motor[rightMotor] = speed;
      motor[leftMotor] = 0;
  }

  // Turning right, stop right motor and power up left.
  if (direction == 1)
  {
     motor[rightMotor] = 0;
     motor[leftMotor] = speed;
  }

   while (turned <= degree)
   {
      traveled = (SensorValue[leftEncoder] + SensorValue[rightEncoder]) * DistancePerTick;
      turned = (traveled / TurningCircle);
      wait1Msec(SleepTime);
   }
}