compCodev2.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    gyro,           sensorGyro)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           leftEDrive,    tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_2)
#pragma config(Motor,  port3,           leftDrive,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           rightEDrive,   tmotorVex393HighSpeed_MC29, PIDControl, reversed, encoderPort, I2C_3)
#pragma config(Motor,  port7,           rightDrive,    tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           lift,          tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port9,           slaveLift,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* MOTOR LAYOUT
----------------
Port 2: Left Encoder
Port 3: Left Drive
Port 6: Right Encoder
Port 7: Right Drive
Port 8: PE A Left Lift
Port 9: PE B Right Lift
*/

//Ctrl + F "TEST" for things to check during testing.

//Global Constants
const float TICKS_PER_INCH = 28.86; //TEST
const float TICKS_PER_DEGREE = 1.5138; //Rotated 10.33 times, so average/(10.33*360)
const float INCHES_PER_TILE = 24.25; //TEST
const float TICKS_PER_TILE = 455;
const float GYRO_SCALING_FACTOR = 1.0645; //TEST
const int PID_DRIVE_MAX = 80;
const int PID_DRIVE_MIN = 20; //TEST
const int PID_ROTATE_MAX = 50;
const int PID_ROTATE_MIN = 30; //TEST
const int MIN_POWER_TO_MOVE = 25;

//Function specific constants
//MoveStraight
const float kp_wheels = -0.1489; //TEST for independant speed control. decrease if drifting to the left
const float kp_drive = 0.0159; //TEST for distance control.

//rotateEncoders
const float kp_rotate = 0.024; //TEST for rotational control.
const float kp_rotateWheels = 0.063;

//rotateGyro
const float kp_rotateGyro = 0.03;

const int threshold = 10; //Error threshold
const int driveThreshold = 20;//4
const int gyroThreshold = 20; //+- degree threshold*10;




/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VEX EDR                      */
/*                                                                           */
/*---------------------------------------------------------------------------*/

// This code is for the VEX cortex platform
#pragma platform(VEX2)

// Select Download method as "competition"
#pragma competitionControl(Competition)

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

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

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

	// Set bDisplayCompetitionStatusOnLcd to false if you don't want the LCD
	// used by the competition include file, for example, you might want
	// to display your team name on the LCD in this function.
	// bDisplayCompetitionStatusOnLcd = false;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
	slaveMotor(slaveLift,lift);
	slaveMotor(leftDrive,leftEDrive);
	slaveMotor(rightDrive,rightEDrive);
	nMotorEncoder[leftEDrive]=0;
	nMotorEncoder[rightEDrive]=0;
	SensorType[gyro]=sensorNone;
	wait1Msec(1000);
	SensorType[gyro]=sensorGyro;
	wait1Msec(2000);
}

void initialize(){
	nMotorEncoder[leftEDrive]=0;
	nMotorEncoder[rightEDrive]=0;
	wait1Msec(200);
}


/* PARAMETERS
-------------------
i: DIRECTION. 1 is forward, -1 is backwards
d: DISTANCE. input is in tiles, can accept decimal values, calculations for ticks is made in the function
*/
void moveStraight(int i, float d){

	//Variables
	float leftPower, rightPower;
	float targetTicks, distErrorL, distErrorR;
	float wheelDiff; //Keeps both sides at the same speed.


	//Clear encoder values
	initialize();

	clearTimer(T1);
	//targetTicks=d*TICKS_PER_TILE;
	targetTicks=1500;
	distErrorL = targetTicks-abs(nMotorEncoder[leftEDrive]);
	distErrorR = targetTicks-abs(nMotorEncoder[rightEDrive]);

	//ACTUAL P LOOP
	//Breaks after 4 seconds or if both left and right side of drive reached the target
	while(time1[T1]<4000 && (abs(distErrorL)>threshold || abs(distErrorR)>threshold)){
		distErrorL = targetTicks-abs(nMotorEncoder[leftEDrive]);
		distErrorR = targetTicks-abs(nMotorEncoder[rightEDrive]);
		wheelDiff = abs(nMotorEncoder[leftEDrive])-abs(nMotorEncoder[rightEDrive]);

		//Proportional power gain
		leftPower=PID_DRIVE_MIN+(kp_drive*distErrorL);
		rightPower=PID_DRIVE_MIN+(kp_drive*distErrorR);

		//Keeps the left power values within the max-min range
		if(leftPower>PID_DRIVE_MAX){
			leftPower=PID_DRIVE_MAX-(wheelDiff*kp_wheels);
			} else {
			leftPower=leftPower-(wheelDiff*kp_wheels);
		}

		//Keeps the right power values within the max-min range
		if(rightPower>PID_DRIVE_MAX){
			rightPower=PID_DRIVE_MAX+(wheelDiff*kp_wheels);
			} else {
			rightPower=rightPower+(wheelDiff*kp_wheels);
		}

		motor[leftEDrive]=leftPower;
		motor[rightEDrive]=rightPower;
		wait1Msec(25); //Run at 50Hz
	}
	motor[leftEDrive]=0;
	motor[rightEDrive]=0;
}



/* PARAMETERS
-------------------
i: DIRECTION. 1 is ccw, -1 is cw.
d: DEGREES. Self explanitory
*/
void rotateEncoders(int i, float d){

	//Variables
	float leftPower, rightPower;
	float targetTicks, rotateErrorL, rotateErrorR, wheelDiff;

	initialize();
	clearTimer(T1);

	//targetTicks=TICKS_PER_DEGREE*d;
	targetTicks=1000;
	rotateErrorL = targetTicks-abs(nMotorEncoder[leftEDrive]);
	rotateErrorR = targetTicks-abs(nMotorEncoder[rightEDrive]);

	//ACTUAL P LOOP
	//Breaks after 4 seconds or if both left and right side of drive reached the target
	while(time1[T1]<4000 && (abs(rotateErrorL)>threshold || abs(rotateErrorR)>threshold)){
		rotateErrorL = targetTicks-abs(nMotorEncoder[leftEDrive]);
		rotateErrorR = targetTicks-abs(nMotorEncoder[rightEDrive]);
		wheelDiff = abs(nMotorEncoder[leftEDrive])-abs(nMotorEncoder[rightEDrive]);

		//Proportional power gain, Minh added: "-(wheelDiff*kp_rotateWheels)" and "+(wheelDiff*kp_rotateWheels)" to leftPower and rightPower
		leftPower=i*(PID_ROTATE_MIN+(kp_rotate*rotateErrorL)-(wheelDiff*kp_rotateWheels));
		rightPower=(-i)*(PID_ROTATE_MIN+(kp_rotate*rotateErrorR)+(wheelDiff*kp_rotateWheels));//TEST figure out +- values for i corresponding to motors

		//Keeps the left power values within the max-min range
		if(leftPower>PID_ROTATE_MAX){
			leftPower=PID_ROTATE_MAX-(wheelDiff*kp_rotateWheels);
			} else {
			leftPower=leftPower-(wheelDiff*kp_rotateWheels);
		}

		//Keeps the right power values within the max-min range
		if(rightPower>PID_ROTATE_MAX){
			rightPower=PID_ROTATE_MAX+(wheelDiff*kp_rotateWheels);
			} else {
			rightPower=rightPower+(wheelDiff*kp_rotateWheels);
		}

		motor[leftEDrive]=leftPower;
		motor[rightEDrive]=rightPower;
		wait1Msec(25); //Run at 50Hz
	}
	motor[leftEDrive]=0;
	motor[rightEDrive]=0;
}

/* PARAMETERS
-------------------
i: DIRECTION. 1 is ccw, -1 is cw.
d: DEGREES. Self explanitory
*/
void rotateGyro(int i, float d){

	//Clear Gyro
	SensorValue[gyro]=0;

	//Variables
	float leftPower, rightPower;
	float targetTicks = d*10*GYRO_SCALING_FACTOR;
	float gyroError = targetTicks-abs(SensorValue[gyro]);

	clearTimer(T1);

	//ACTUAL P LOOP
	//Breaks after 4 seconds or if both left and right side of drive reached the target
	while(time1[T1]<4000 && abs(gyroError)>gyroThreshold){
		gyroError=targetTicks-abs(SensorValue[gyro]);

		//Proportional power gain
		leftPower=i*(PID_ROTATE_MIN+(kp_rotateGyro*gyroError);
		rightPower=(-i)*(PID_ROTATE_MIN+(kp_rotateGyro*gyroError);

		//Keeps the left power values within the max-min range
		if(leftPower>PID_ROTATE_MAX){
			leftPower=PID_ROTATE_MAX;
			} else {
			//leftPower=leftPower-(wheelDiff*kp_rotateWheels);
		}

		//Keeps the right power values within the max-min range
		if(rightPower>PID_ROTATE_MAX){
			rightPower=PID_ROTATE_MAX
			} else {
			//rightPower=rightPower+(wheelDiff*kp_rotateWheels);
		}

		motor[leftEDrive]=leftPower;
		motor[rightEDrive]=rightPower;
		wait1Msec(25); //Run at 50Hz
	}
	motor[leftEDrive]=0;
	motor[rightEDrive]=0;
}

void internalPID(int i, float d, int s){
	//float targetTicks=i*d*TICKS_PER_TILE;
	float targetTicks=1000;
	initialize();

	setMotorTarget(leftEDrive, targetTicks, s, false);
	setMotorTarget(rightEDrive, targetTicks, s, false);
	waitUntilMotorStop(leftEDrive);
	waitUntilMotorStop(rightEDrive);
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/

task autonomous()
{
	//moveStraight(1,3);
	//rotateGyro(1,360);
	internalPID(1,2,50);
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              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.   */
/*---------------------------------------------------------------------------*/

task usercontrol()
{
	initialize();
	while(1){
		if(abs(vexRT[Ch3])>driveThreshold){
			motor[leftEDrive]=vexRT[Ch3];
			}	else {
			motor[leftEDrive]=0;
		}
		if(abs(vexRT[Ch2])>driveThreshold){
			motor[rightEDrive]=vexRT[Ch2];
			}	else {
			motor[rightEDrive]=0;
		}
		if(vexRT[Btn6U]==1){
				motor[lift]=127;
		}
		else if(vexRT[Btn6D]==1){
				motor[lift]=-127;
		}
		else {
			motor[lift]=0;
		}
	}
}