#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Sensor, S2,     HTGYRO,              sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     HTIRS1,              sensorI2CCustom)
#pragma config(Sensor, S4,     HTIRS2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     Ldrive,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     Rdrive,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     BallVac,       tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    LeftGate,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    RightGate,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    CrateFlip,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Gate,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    RBC,                  tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    RFC,                  tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "C:\Users\AlecG\Desktop\Robot-C\Includes\auxiliaryfunctions.h"
#include "C:\Users\AlecG\Desktop\Robot-C\Includes\common.h"
#include "C:\Users\AlecG\Desktop\Robot-C\Includes\HTIRS2-driver.h"
#include "C:\Users\AlecG\Desktop\Robot-C\rdpartyrobotcdr-v1.8.1-HT\drivers\HTGYRO-driver.h"
//D is left drive E is Right


float speedValue = 0;



 ///////////////////////////////////////////////////////////////////////////////////////////////
// This whole function is designed to move the robot a defined distance precisely. It also converts inches to encoder counts.
void MoveBump(int inDist, int power)
{
 float EncoderCounts = (inDist - 1.9364)/0.0064; //This is all the data that Alec took last year put into an equation.
 inDist = EncoderCounts; // Floats screw up all the things. so we used inDist but made it hold the encoder value.
 //writeDebugStreamLine( "%d",inDist );
 while(abs(nMotorEncoder[motorE]) < inDist && abs(nMotorEncoder[motorD]) < inDist)
 {
   writeDebugStreamLine("%f" ,SpeedCheck(Ldrive, 0));
   motor[Ldrive] = power;
   motor[Rdrive] = power;
 }
 motor[Ldrive] = 0;
 motor[Rdrive] = 0;
 wait10Msec(1); // just to let the encoders reset when the robot is still
 nMotorEncoder[Ldrive] = 0;
 nMotorEncoder[Rdrive] = 0;
}


/* Function returns the speed of a Tetrix DC motor equipped with encoder in RPMs.

Inputs to the function are the motor you want to check, and an index.  Use a different index for every
motor you are checking.

*/

 task main()
 {
   while(true)
   {
     motor[Ldrive] = 50;
     motor[Rdrive] = 50;
     speedValue = SpeedCheck(Ldrive, 0);
     if (speedValue != 0)
     {
       writeDebugStreamLine ("%f", speedValue);
     }
     //writeDebugStreamLine("%f" ,SpeedCheck(Ldrive, 0));
     //MoveBump(100, 50);// this is the speed (50) of our Blue and Red 20 autonomous programs
     //writeDebugStream("alec");
     wait1Msec(10);
   }
 }