armLift.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(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           backR,         tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor,  port3,           frontR,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           frontL,        tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           backL,         tmotorVex393TurboSpeed_MC29, PIDControl, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port6,           leftForkLift,  tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor,  port7,           rightForkLift, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           armLift,       tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_4)
#pragma config(Motor,  port9,           claw,          tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_5)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// ASSUMING WHEEL ENCODER ARE THE BACK OF BOT
float error = 0;
int power = 0;
float armCurrentValue = 0;
float integral = 0;
float range = -1681;
float Kp = 14; 	//4.6	30 	30
float Kd = 60;	//1 	50	60
float Ki = 1;		//0.3 0.6 1
float target = 0;
float lastError = 0;
float derivative = 0;
float integralActiveZone = 100;
float integralLimits = 2000; // 50/Ki
float armToFront = -970; //1316, 1300
float armStraightUp = -825;
float armToBack = -281; //-350 -365 -282
float armToBack2 = -245;
float armToBack3 = -165;
float clawShouldBeCLose = -200;
float clawKp = 0.5;
float clawCurrentValue = 0;
float clawError = 0;
float clawTarget = 0;

float forkKp = 1.5;
float forkRange = 980;
float forkDown = 100; //91
float forkCurrentValue = 0;
float forkError = 0;
float forkTarget = 0;

void initialize(){
	resetMotorEncoder(leftForkLift);
	resetMotorEncoder(armLift);
	resetMotorEncoder(claw);
}

task armPidControl(){
	while (true){
		armCurrentValue = getMotorEncoder(armLift);
		error = target - armCurrentValue;
		if (error != 0)
		{
			derivative = error-lastError;
		}
		else {
			lastError = 0;
			derivative = 0;
			integral = 0;
		}

		if (abs(error) < integralActiveZone){
			integral = integral + error;
			if (abs(error) <= 2){
			integral = 0;
			}
		}
		else {
			integral = 0;
		}
		if (abs(integral) > integralLimits){
			integral = integralLimits;
		}
		power = (int)((Kp*error + Kd*derivative + Ki*integral)*(127/range));
		motor[armLift] = power;
		wait1Msec(20);
		lastError = error;
	}
}

float clawRange = 30; //20
float clawClose = 3; //8
void clawOpen(){
	motor[claw] = 127;
	wait1Msec(800);
}

void clawClosing(){
	motor[claw] = 0;
}

task forkLiftPID(){
	while (true){
		forkCurrentValue = getMotorEncoder(leftForkLift);
		forkError = forkTarget - forkCurrentValue;
		if (abs(forkError) <= 10){
			forkError = 0;
		}


		motor[leftForkLift] = (int)((forkKp*forkError)*(127/forkRange));
		motor[rightForkLift] = (int)((forkKp*forkError)*(127/forkRange));
		wait1Msec(20);
	}
}

task main(){
	initialize();
	startTask(armPidControl);
	startTask(forkLiftPID);
		while (true){
			target = armToBack;
			clawOpen();
			clawClosing();
			wait1Msec(1000);
			target = armToFront;
			wait1Msec(1000);
			clawOpen();
			clawClosing();
			wait1Msec(500);
		}
}