#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);
}
}