autoFunctions.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#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, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port3,           leftDrive,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           rightEDrive,   tmotorVex393HighSpeed_MC29, openLoop, 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.57; //Rotated 10.33 times, so average/(10.33*360)
const float INCHES_PER_TILE = 24.25; //TEST
const float TICKS_PER_TILE = TICKS_PER_INCH*INCHES_PER_TILE;
const int PID_DRIVE_MAX = 80;
const int PID_DRIVE_MIN = 20; //TEST
const int PID_ROTATE_MAX = 50;
const int PID_ROTATE_MIN = 25; //TEST
const int MIN_POWER_TO_MOVE = 25;

const float kp_wheels = 2; //TEST for independant speed control.
const float kp_drive = 0.0168; //TEST for distance control.
const float kp_rotate = 0.05; //TEST for rotational control.
const int threshold = 10; //Error threshold




//Only use for debugging and testing.
void testLoop(){
	motor[leftEDrive]=MIN_POWER_TO_MOVE;
	motor[rightEDrive]=-MIN_POWER_TO_MOVE;
	wait1Msec(14500);
	motor[leftEDrive]=0;
	motor[rightEDrive]=0;
}




void initialize(){
	slaveMotor(slaveLift,lift);
	slaveMotor(leftDrive,leftEDrive);
	slaveMotor(rightDrive,rightEDrive);
	nMotorEncoder[leftEDrive]=0;
	nMotorEncoder[rightEDrive]=0;
	wait1Msec(1000);
}




/* 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
	nMotorEncoder[leftEDrive]=0;
	nMotorEncoder[rightEDrive]=0;
	wait1Msec(200);

	clearTimer(T1);
	//targetTicks=d*TICKS_PER_TILE;
	targetTicks=2000;
	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)){
		//targetTicks=d*TICKS_PER_TILE;
		targetTicks=2000;
		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 rotate(int i, float d){

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


	//Clear encoder values
	nMotorEncoder[leftEDrive]=0;
	nMotorEncoder[rightEDrive]=0;
	wait1Msec(200);

	clearTimer(T1);

	targetTicks=TICKS_PER_DEGREE*d;
	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)){
		targetTicks=TICKS_PER_DEGREE*d;
		rotateErrorL = targetTicks-abs(nMotorEncoder[leftEDrive]);
		rotateErrorR = targetTicks-abs(nMotorEncoder[rightEDrive]);
		wheelDiff = abs(nMotorEncoder[leftEDrive])-abs(nMotorEncoder[rightEDrive]);

		//Proportional power gain
		leftPower=i*(PID_ROTATE_MIN+(kp_rotate*rotateErrorL));
		rightPower=(-i)*(PID_ROTATE_MIN+(kp_rotate*rotateErrorR));//TEST figure out +- values for i corresponding to motors

		//Keeps the left power values within the max-min range
		if(abs(leftPower)>PID_ROTATE_MAX){
			leftPower=i*(PID_ROTATE_MAX-(wheelDiff*kp_wheels));//TEST for i
		}

		//Keeps the right power values within the max-min range
		if(abs(rightPower)>PID_ROTATE_MAX){
			rightPower=(-i)*(PID_ROTATE_MAX+(wheelDiff*kp_wheels));//TEST for i
		}

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




task main()
{
	initialize();
	//testLoop();
	//rotate(1,360);
	moveStraight(1,3);
}