CRHSCompetetive.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  blueSide1,      sensorDigitalIn)
#pragma config(Sensor, dgtl6,  blueSide2,      sensorDigitalIn)
#pragma config(Sensor, dgtl7,  blueBack1,      sensorDigitalIn)
#pragma config(Sensor, dgtl8,  blueBack2,     sensorDigitalIn)
#pragma config(Sensor, dgtl9,  redSide1,       sensorDigitalIn)
#pragma config(Sensor, dgtl10, redSide2,       sensorDigitalIn)
#pragma config(Sensor, dgtl11, redBack1,       sensorDigitalIn)
#pragma config(Sensor, dgtl12, redBack2,       sensorNone)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           armMotor,      tmotorVex393_HBridge, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port2,           clawMotor,     tmotorVex393_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port3,           backRight,     tmotorVex269_MC29, openLoop, reversed, encoderPort, dgtl1)
#pragma config(Motor,  port4,           backLeft,      tmotorVex269_MC29, openLoop, encoderPort, dgtl3)
#pragma config(Motor,  port5,           frontRight,    tmotorVex269_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           frontLeft,     tmotorVex269_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Beware if you enable PID control...... It hasn't always ended well for the unexperienced programmer

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

//This sets the time for doing certain moves if the encoders aren't working
//float feet10 = 7130; //7.13 seconds
//float completeTurn = 4680; //4.68 seconds

//29.85 is how many inches needed to turn for a 180
//float rotationsRight = distanceRight / circumference;
//float degreesRight = rotationsRight * 360;
//int encoderCountsRight = degreesRight * 4; // The encoder counts in quarter degrees.

// This is so we can find out how many times the left wheels have to turn to go the specefied distance

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts

}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

//Declaring an array to make setting motor speed in autonomous easier
string motors[] = {frontRight, frontLeft, backRight, backLeft};

// 5.125 makes us innacurate, how about 5.25? (Check this later down the road)
float distanceRight, distanceLeft, rotationsRight, rotationsLeft, degreesRight, degreesLeft, ratio, diameter = 5.125, circumference = PI * diameter;
int encoderCountsRight = 0, encoderCountsLeft = 0;

void ForwardForDistance(int speed, float targetDistance) //The arguments are sorted alphabetically. Make speed negative to reverse and change the wait to abs to make sure it works
{
	//float rotations = targetDistance / circumference; On torque mode (stock), there are 627 counts to a revolution, high speed there is 392, and turbo speed has 261
	//float degrees = rotations * 360;
	//float encoderCounts = degrees * 4;	// 24 / 18.375 is 1.30612244898

	//Auto-correction
	//while(nMotorEncoder[frontRight] < encoderCounts && nMotorEncoder[frontLeft] < encoderCounts)
	//{
	//	if (nMotorEncoder[frontRight] > nMotorEncoder[frontLeft])
	//	{
	//		motor[frontRight] = speed / 2;
	//		motor[backRight] = speed / 2;
	//	}
	//	else if(nMotorEncoder[frontRight] < nMotorEncoder[frontLeft])
	//	{
	//		motor[frontLeft] = speed / 2; // These are negative because we are still turning
	//		motor[backLeft] = speed / 2;
	//	}
	//	else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
	//	{
	//		motor[frontRight] = speed;
	//		motor[frontLeft] = speed;
	//		motor[backRight] = speed;
	//		motor[backLeft] = speed;
	//	}
	//}



	float motorEncoderRight = targetDistance, motorEncoderLeft = targetDistance;	//600 = 1 foot

	//while (nMotorEncoder(frontRight) < motorEncoderRight && nMotorEncoder(frontLeft) < motorEncoderLeft)
	//{
	//	motor[frontRight] = speed;
	//	motor[frontLeft] = speed;
	//	motor[backRight] = speed;
	//	motor[backLeft] = speed;
	//}

	//Could this work???
	while (nMotorEncoder(frontRight) < motorEncoderRight && nMotorEncoder(frontLeft) < motorEncoderLeft)
	{
		for (int i = 0; i <= sizeof(motors); i++)
			motor[motors[i]] = speed;
	}
}

//Could this work as well???
void Turn(float degreesToTurn, int speed)
{
	ratio = 180 / degreesToTurn;
	distanceRight = 29.85 / ratio;

	rotationsRight = distanceRight / circumference;
	degreesRight = rotationsRight * 360;

	encoderCountsRight = degreesRight * 4;
	encoderCountsLeft = encoderCountsRight * -1;

	if (degreesToTurn < 0)
	{
		motor[frontRight] = speed;
		motor[frontLeft] = speed * -1;
		motor[backRight] = speed;
		motor[backLeft] = speed * -1;

		while (nMotorEncoder[frontRight] < encoderCountsRight && nMotorEncoder[frontLeft] < encoderCountsLeft)
		{
			if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
			{
				motor[frontRight] = speed / 2;
				motor[backRight] = speed / 2;
			}
			else if (abs(nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
			{
				motor[frontLeft] = speed / -2;
				motor[backLeft] = speed / -2;
			}
			else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
			{
				motor[frontRight] = speed;
				motor[frontLeft] = speed;
				motor[backRight] = speed;
				motor[backLeft] = speed;
			}
		}
	}

	else if (degreesToTurn > 0)
	{
		motor[frontRight] = speed * -1;
		motor[frontLeft] = speed;
		motor[backRight] = speed * -1;
		motor[backLeft] = speed;

		while (nMotorEncoder[frontRight] < encoderCountsRight && nMotorEncoder[frontLeft] < encoderCountsLeft)
		{
			if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
			{
				//Since one value is negative and one is positive, abs(absolute value) makes it so that we can make sure the robot turns evenly
				motor[frontRight] = speed / -2;
				motor[backRight] = speed / -2;
			}
			else if (abs(nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
			{
				motor[frontLeft] = speed / 2; // These are negative because we are still turning
				motor[backLeft] = speed / 2;
			}
			else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
			{
				motor[frontRight] = speed;
				motor[frontLeft] = speed;
				motor[backRight] = speed;
				motor[backLeft] = speed;
			}
		}
	}
}

/*void TurnLeft(float degreesToTurn, int speed)
{
	//float rotationsRight = distanceRight / circumference; On torque mode (stock), there are 627 counts to a revolution, high speed there is 392, and
	// turbo speed has 261
	//float degreesRight = rotationsRight * 627.2;
	//float encoderCountsRight = degreesRight * 4; // 24 / 18.375 is 1.30612244898
	////float rotationsLeft = distanceLeft / circumference;
	//float rotationsLeft = rotationsRight * -1; // This is the same as calculating the rotations directly
	//float degreesLeft = rotationsLeft * 627.2;
	//float encoderCountsLeft = degreesLeft * 4;

	ratio = 180 / degreesToTurn; // We find the ratio of degrees to turn to doing a 180 turn
	distanceLeft = 29.85 //ratio; This then compares the distance to turn of a 180 to the ratio made above to make the distance the robot has to turn

	rotationsLeft = distanceLeft / circumference;
	degreesLeft = rotationsLeft * 360;
	encoderCountsLeft = degreesLeft * 4; // The encoder counts in quarter degrees.
	encoderCountsRight = encoderCountsLeft * -1; // This is the same as calculating the counts directly, but saves time :)

	motor[frontRight] = speed * -1;
	motor[frontLeft] = speed;
	motor[backRight] = speed * -1;
	motor[backLeft] = speed;

	while(nMotorEncoder[frontRight] < encoderCountsRight && nMotorEncoder[frontLeft] < encoderCountsLeft)
	{
		if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
		{ Since one value is negative and one is positive, abs (absolute value) makes it so that we can make sure the robot
			turns evenly
			motor[frontRight] = speed / -2;
			motor[backRight] = speed / -2;
		}
		else if(abs(nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
		{
			motor[frontLeft] = speed / 2; // These are negative because we are still turning
			motor[backLeft] = speed / 2;
		}
		else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
		{
			motor[frontRight] = speed;
			motor[frontLeft] = speed;
			motor[backRight] = speed;
			motor[backLeft] = speed;
		}
	}
}

void TurnRight(float degreesToTurn, int speed)
{
	ratio = 180 / degreesToTurn;
	distanceRight = 29.85 / ratio;

	rotationsRight = distanceRight / circumference;
	degreesRight = rotationsRight * 360;
	encoderCountsRight = degreesRight * 4;

	encoderCountsLeft = encoderCountsRight * -1;

	resetMotorEncoder(frontRight);
	resetMotorEncoder(frontLeft);

	motor[frontRight] = speed;
	motor[frontLeft] = speed * -1;
	motor[backRight] = speed;
	motor[backLeft] = speed * -1;

	while(nMotorEncoder[frontRight] < encoderCountsRight && nMotorEncoder[frontLeft] < encoderCountsLeft)
	{
		if (abs(nMotorEncoder[frontRight]) > abs(nMotorEncoder[frontLeft]))
		{
			motor[frontRight] = speed / 2;
			motor[backRight] = speed / 2;
		}
		else if(abs(nMotorEncoder[frontRight]) < abs(nMotorEncoder[frontLeft]))
		{
			motor[frontLeft] = speed / -2;
			motor[backLeft] = speed / -2;
		}
		else if (nMotorEncoder[frontRight] == nMotorEncoder[frontLeft])
		{
			motor[frontRight] = speed;
			motor[frontLeft] = speed;
			motor[backRight] = speed;
			motor[backLeft] = speed;
		}
	}
}*/

void Wait(float timeToWait)
{
	wait1Msec(timeToWait * 1000);
}

void Stop()
{
	motor[frontRight] = 0;
	motor[frontLeft] = 0;
	motor[backRight] = 0;
	motor[backLeft] = 0;
}

task autonomous()
{

	// These 'if' statements check to see if there is a jumper in the selected port. If there is a jumper in that port, the sensor returns a false value that portion of code will be ran.

	//float rotationsRight = distanceRight / circumference; // On torque mode (stock), there are 627 counts to a revolution, high speed there is 392, and
	// turbo speed has 261
	//float degreesRight = rotationsRight * 627.2;
	//float encoderCountsRight = degreesRight * 4; // 24 / 18.375 is 1.30612244898
	//float rotationsLeft = distanceLeft / circumference;
	//float degreesLeft = rotationsLeft * 627.2;
	//float encoderCountsLeft = degreesLeft * 4;

	if (!SensorBoolean[blueSide1])
	{

	}

	else if (!SensorBoolean[blueSide2])
	{

	}

	else if (!SensorBoolean[blueBack1])
	{

	}

	else if (!SensorBoolean[blueBack2])
	{

	}

	else if (!SensorBoolean[redSide1])
	{

	}

	else if (!SensorBoolean[redSide2])
	{

	}

	else if (!SensorBoolean[redBack1])
	{

	}

	else if (!SensorBoolean[redBack2])
	{

	}

	else
	{

	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

int _threshold = 20;

task usercontrol()
{
	while (true)
	{

		// This is the main execution loop for the user control program. Each time through the loop
		// your program should update motor + servo values based on feedback from the joysticks.
		// Based on what wheels are installed, change the program between regular and mecanum wheels

		/*frontRightValue = 0;
		backRightValue = 0;
		frontLeftValue = 0;
		backLeftValue = 0;

		if (abs(vexRT[Ch2]) > 10)
		{
				frontRightValue = vexRT[Ch2];
				backRightValue = vexRT[Ch2];
		}
		if (abs(vexRT[Ch3]) > 10)
		{
				frontLeftValue = vexRT[Ch3];
				backLeftValue = vexRT[Ch3];
		}
		if (abs(vexRT[Ch2Xmtr2]) > 10)
		{
				frontRightValue = vexRT[Ch2Xmtr2];
				backRightValue = vexRT[Ch2Xmtr2];
		}
		if (abs(vexRT[Ch3Xmtr2]) > 10)
		{
				frontLeftValue = vexRT[Ch3Xmtr2];
				backLeftValue = vexRT[Ch3Xmtr2];
		}

		motor[frontRight] = frontRightValue;
		motor[backRight] = backRightValue;
		motor[frontLeft] = frontLeftValue;
		motor[backLeft] = backLeftValue;*/

		// Under here is the code for mecanum wheels
		//Create "deadzone" variables. Adjust threshold value to increase/decrease deadzone
	  int X2 = 0, Y1 = 0, X1 = 0, threshold = 15, armMovement = 0, clawMovement = 0;

    //Create "deadzone" for Y1/Ch3
    if (abs(vexRT[Ch3]) > threshold)
      Y1 = vexRT[Ch3];
    else if (abs(vexRT[Ch3Xmtr2]) > threshold)
    	Y1 = vexRT[Ch3Xmtr2];
    else
      Y1 = 0;
    //Create "deadzone" for X1/Ch4
    if (abs(vexRT[Ch4]) > threshold)
      X1 = vexRT[Ch4];
    else if (abs(vexRT[Ch4Xmtr2]) > threshold)
    	X1 = vexRT[Ch4Xmtr2];
    else
      X1 = 0;
    //Create "deadzone" for X2/Ch1
    if (abs(vexRT[Ch1]) > threshold)
      X2 = vexRT[Ch1];
    else if (abs(vexRT[Ch1Xmtr2]) > threshold)
    	X2 = vexRT[Ch1Xmtr2];
    else
      X2 = 0;

    //Arm Controls
    if (vexRT[Btn5D] == 1 || vexRT[Btn5DXmtr2] == 1)
    	armMovement = 127;
   	else if (vexRT[Btn5U] == 1 || vexRT[Btn5UXmtr2] == 1)
   		armMovement = -127;

  	//Claw controls
   	if (vexRT[Btn6D] == 1 || vexRT[Btn6DXmtr2] == 1)
    	clawMovement = 127;
   	else if (vexRT[Btn6U] == 1 || vexRT[Btn6UXmtr2] == 1)
   		clawMovement = -127;

    //Remote Control Commands
    motor[frontRight] = Y1 - X2 - X1;
    motor[backRight] =  Y1 - X2 + X1;
    motor[frontLeft] = Y1 + X2 + X1;
    motor[backLeft] =  Y1 + X2 - X1;
    motor[armMotor] = armMovement;
    motor[clawMotor] = clawMovement;
	}
}