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

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 = -1316; //1316, 1300
float armToFront_One = -1247;
float armToFront_Three = -1426;
float armToFront_Four = -1505;
float armStraightUp = -825;
float armToBack = -350;

float clawKp = 0.5;
float clawRange = 27;
float clawClose = 0;
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 gyroinit() {
	SensorType[gyro] = sensorNone;
	wait1Msec(300);
	SensorType[gyro] = sensorGyro;
	wait1Msec(2000);
}

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

void rotateBot(int anglex10){
	float startAngle = SensorValue[in1];
	//perfect values - p: .9, i - 0, d:4.4 (speed at .7)
	float kp = 0.9;
	float ki = 0;
	float kd = 4.4;

	float speed = 0;
	int error2 = 1, integral = 0;
	float error = anglex10 - (SensorValue[in1] - startAngle);
	float gyro0 = SensorValue[in1];

	while (abs(error) > 15) {
		error = anglex10 - (SensorValue[in1] - startAngle);
		float speed = 0.7*(error*kp) + ((error - error2)*kd);

		motor[backR] = -speed;
		motor[frontR] = -speed;
		motor[backL] = speed;
		motor[frontL] = speed;

		error2 = error;
		wait1Msec(20);
	}
	motor[backR] = 0;
	motor[frontR] = 0;
	motor[backL] = 0;
	motor[frontL] = 0;
}

task manualControl(){
	int threshold=30;
	while(1){
		if(abs(vexRT[Ch3])>threshold || abs(vexRT[Ch2])>threshold){
			if(abs(vexRT[Ch3])>threshold){
				motor[frontL]=vexRT[Ch3];
				motor[backL]=vexRT[Ch3];
			}
			if(abs(vexRT[Ch2])>threshold){
				motor[frontR]=vexRT[Ch3];
				motor[backR]=vexRT[Ch3];
			}
		}
		else{
			motor[frontL]=0;
			motor[backL]=0;
			motor[frontR]=0;
			motor[backR]=0;
		}
		if(vexRT[Btn8D]==1){
			clawTarget=clawRange;
		}
		if(vexRT[Btn8U]==1){
			clawTarget=clawClose;
		}
		if(vexRT[Btn6U]==1){
			forkTarget=forkRange;
		}
		if(vexRT[Btn6D]==1){
			forkTarget=forkDown;
		}
		if(vexRT[Btn5U]==1){
			if(target<=armToBack){
				target+=1;
			}
			else{
				target=armToBack;
			}
		}
		if(vexRT[Btn5D]==1){
			if(target>=armToFront){
				target+=1;
			}
			else{
				target=armToFront;
			}
		}
	}
}

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))+ 10;
		motor[armLift] = power;
		wait1Msec(20);
		lastError = error;
	}
}

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 theClawPControl(){
	while (true){
		//clawCurrentValue = getMotorEncoder(claw);
		//clawError = clawTarget - clawCurrentValue;

		/*if (abs(clawError) <= 1){
		clawError = 0;
		}*/

		//motor[claw] = ((int)((clawKp*clawError)*(127/clawRange))) + 10*(clawError/(abs(clawError)+0.1));
		//motor[claw] = (int)((clawKp*clawError)*(127/clawRange)-40);
		if (clawTarget > 0){
			motor[claw] = 70;
		}
		else if (clawTarget == 0){
			motor[claw] = -70;
		}
		else {
			motor[claw] = 0;
		}
		wait1Msec(20);
	}
}

void auto(){
	//combinations of different PID targets; runs through once
}

task main(){
	initialize();
	startTask(manualControl);
	startTask(forkLiftPID);
	startTask(theClawPControl);
	startTask(armPidControl);
	while(true){
		if(vexRT[Btn7D]==1){
			auto();
		}
	}
}