finalCompCode.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in8,    gyro,           sensorGyro)
#pragma config(Sensor, dgtl1,  touch,          sensorTouch)
#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,           LEDrive,       tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port4,           fork,          tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           L_lift,        tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor,  port6,           R_lift,        tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_4)
#pragma config(Motor,  port7,           REDrive,       tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port8,           claw,          tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           clawArm,       tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_5)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX2)
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes.c"


//   __        __   __                          __          __        ___  __
//  / _` |    /  \ |__)  /\  |       \  /  /\  |__) |  /\  |__) |    |__  /__`
//  \__> |___ \__/ |__) /~~\ |___     \/  /~~\ |  \ | /~~\ |__) |___ |___ .__/
//

float kP_drive = 2; //2
float kP_drift = 10; //10
float kD = 16; //15.6
float kI = 0.36; //0.36
float toMotor = 0;
float powerP = 0;
float powerI = 0;
float powerD = 0;

float headingAngle = 0;
int setTime = 0;

//Conversions
float inches_per_tile = 24; //23.25, 22.25
float ticks_per_revolution = 392; //FIX This is for the high speed configuration only
float ticks_per_inch = ticks_per_revolution/(PI*4); //FIX 4" omni wheels
float ticks_per_tile = ticks_per_inch*inches_per_tile;

//moveStraight variables
float error;
float power;
float driftPower;
float targetTicks;
float prevError;
float gyroError;
float derivative;
float integral = 0;
float integral_active_zone = 45; //FIX idk man arbitrary, 130, 40
float errorThreshold = 2; //errorThresholdInTicks=(ticks_per_inch)*(errorThresholdInInches) arbitrary

//Slew Rate
int motorSlewRate = 5; //20
float tempMotor = 0;
float motorReq[2];

// + counter clockwise
// - clockwise
// angle in degree
//float turn_kP = 3; //1, 3
//float turn_kI = 0.7; //0.3
//float turn_kD = 16.7; //15
//float turnError;
//float turn_Power;
//float turn_Integral = 0;
//float turn_Derivative = 0;
//float turn_Proportional = 0;
//float current_Angle;
//float angle;

//turn
const float gyro_ratio=(955.0/900.0); // real world gyro value tune ratio
const float kPt = 0.1; // P tune for turning
const float kDt = 0.08; // D tune for turning
const float kIt = 0.012; // I tune for turning
float Header = 0; // align header with gyro
const float max_turn_spd = 85;
const float max_int = 4000;

//cone
const float kP_cone=2;
const float kI_cone=0;
const float kD_cone=2;
int height=0; //height preset for auton: 1 for preload height, 2 for cones 1-4 drop height, 3 for cones 5-8/9 drop height
int target=0;
int lifterror;
int liftpower;
float armTarget;
int counter=0;
bool floor1 = false;
bool preload=false;


//   __   __   ___              ___  __
//  |__) |__) |__      /\  |  |  |  /  \ |\ |
//  |    |  \ |___    /~~\ \__/  |  \__/ | \|
//

void pre_auton()
{
	nMotorEncoder[LEDrive]=0;
	nMotorEncoder[REDrive]=0;
	slaveMotor(R_lift, L_lift);
	SensorType[gyro] = sensorNone;
	wait1Msec(1000);
	SensorType[gyro] = sensorGyro;
	wait1Msec(2000);
	SensorValue[gyro] = 0;
}



//    __       ___          __      ___  ___
//   /__` |   |__  |  |    |__)  /\  |  |__
//   .__/ |__ |___ |/\|    |  \ /~~\ |  |___
//

void assignPower (int motorIndex, int motorReq_Index){ //REDrive is index 6, LEDrive is index 1
	tempMotor = motor[motorIndex];
	if (tempMotor != motorReq[motorReq_Index])
	{
		if (tempMotor < motorReq[motorReq_Index])
		{
			tempMotor = tempMotor +	motorSlewRate;
			if (tempMotor > motorReq[motorReq_Index])
			{
				tempMotor = motorReq[motorReq_Index];
			}
		}
		else
		{
			tempMotor = tempMotor -	motorSlewRate;
			if (tempMotor < motorReq[motorReq_Index])
			{
				tempMotor = motorReq[motorReq_Index];
			}
		}

		motor[motorIndex] = tempMotor;
	}
}

task slewRate()
{
	while (1)
	{
		assignPower(6, 1); //REDrive is index 6
		assignPower(1, 0); //LEDrive is index 1
		wait1Msec(15);
	}
}


//         __        ___     __  ___  __          __       ___
//   |\/| /  \ \  / |__     /__`  |  |__)  /\  | / _` |__|  |
//   |  | \__/  \/  |___    .__/  |  |  \ /~~\ | \__> |  |  |
//

//if robot is not straight by the end of loop, it will become the new heanding angle
//gyro did not take into account when the sensor value overflow
void moveStraight(int direction, float tiles){
	targetTicks = (tiles*ticks_per_tile) - prevError;

	if (tiles == 1){
		setTime = 2000;
		kP_drive = 2;//2
		kD = 15.7;
		kI = 0.36; //0.36
		if (direction == 1 || direction == 2){
			targetTicks = tiles*(ticks_per_tile - 65);
		}
		if (direction == -1){
			targetTicks = tiles*(ticks_per_tile - 85);
		}
	}

	if (tiles == 2 || tiles == 3){
		setTime = 4000;
		kP_drive = 2; //2
		kD = 15.7; //16
		kI = 0.36; //0.8, 0.36
		if (direction == 1){
			targetTicks = tiles*(ticks_per_tile - 60); //50, 65
		}
		if (direction == -1){
			targetTicks = tiles*(ticks_per_tile - 57); //45
		}
	}
	prevError = targetTicks;
	nMotorEncoder[LEDrive]=0;
	nMotorEncoder[REDrive]=0;
	error = targetTicks-((abs(nMotorEncoder[LEDrive])+abs(nMotorEncoder[REDrive]))/2);
	float scaling = 127/targetTicks;
	integral = 0;
	clearTimer(T1);
	while((time1[T1]<setTime)){ // && (error>errorThreshold)1500
		error = targetTicks-((abs(nMotorEncoder[LEDrive])+abs(nMotorEncoder[REDrive]))/2);
		gyroError = (SensorValue[gyro]/10)-headingAngle; //Error in degrees

		integral = integral + error;
		if(abs(error)>integral_active_zone){
			integral = 0;
		}
		if (abs(error) <= errorThreshold){
			integral = 0;
		}

		derivative = error - prevError;

		driftPower = gyroError * kP_drift;
		powerP = kP_drive*error*direction*scaling;
		powerI = kI*integral*direction*scaling;
		powerD = kD*derivative*direction*scaling;
		power = (powerP)+(powerI)+(powerD);

		if (power > 97){
			power = 97;
		}
		else if (power < -97){
			power = -97;
		}

		if (driftPower > 30){
			driftPower = 30;
		}
		else if (driftPower < -30){
			driftPower = -30;
		}

		if(abs(error)<10){
			driftPower = 0;
		}

		toMotor = power;
		motorReq[0] = toMotor + driftPower; //left
		motorReq[1] = toMotor - driftPower; //right
		prevError = error;
		wait1Msec(25);
	}
	toMotor = 0;
	motorReq[0] = toMotor;
	motorReq[1] = toMotor;
}


//   ___       __
//    |  |  | |__)|\ |
//    |  \__/ |  \| \|
//

void turn (float target, float time_turn)
{
	Header = target*gyro_ratio;
	// initialize values
	float error_turn, prevError =0,integral=0,derivative=0,output_power;// initialize values
	clearTimer(T1);
	while(time1(T1)<time_turn)
	{
		error_turn = (Header) - (SensorValue(gyro)); // calculate left side degrees left to rotate
		// logic examples:
		// Header = 900, Sensor = 0 -> Error starts at 900, left side spins positive, right side spins negative
		// Header = 0, Sensor = 900 -> Error starts at -900, left side spins negative, right side spins positive
		// Header = 900, Gyro = -900 -> Error starts at 1800, left side spins positive, right side spins negative
		// Header = -900, Gyro = 900 -> Error starts at -1800, left side spins negative, right side spins positive
		// Header is 1800, Gyro is 900 -> Error starts at 900, left side spins positive, right side spins negative
		// Header is -1800, Gyro is -900 - > Error starts at -900, left side spins negative, right side spins positive

		if (abs(error_turn)<100)
		integral = error_turn+integral;

		if (integral>max_int)
			integral = max_int;

		if (integral< -max_int)
			integral = -max_int;

		derivative = error_turn - prevError;

		output_power = error_turn*kPt + integral*kIt + derivative*kDt; // calculate output power with PID tuning, direction included

		if (output_power > max_turn_spd)
			output_power = max_turn_spd;

		motor[REDrive]= output_power; // set right drive power
		motor[LEDrive]= -output_power; // set left drive power
		prevError = error_turn; // for derivative tuning
		wait1Msec(25);
		error = error_turn;
	}
	return;
}



//   __   __        ___            ___ ___
//  /  ` /  \ |\ | |__     |    | |__   |
//  \__, \__/ | \| |___    |___ | |     |
//

task coneLift(){
	int error_cone;
	int prevError_cone;
	int integral_cone;
	int derivative_cone;
	int ticks_cone;
	int power_cone;
	bool floor1;
	while(1){
		prevError_cone=0;
		derivative_cone=0;
		switch(height){
			case 1:
				ticks_cone=150;
				break;
			case 2:
				ticks_cone=200;
				break;
			case 3:
				ticks_cone=400;
				break;
			default:
				if(ticks_cone==400){
					ticks_cone=200;
					wait1Msec(500);
				}
				ticks_cone=0;
				break;
		}

		while(abs(getMotorEncoder(R_lift))<abs(ticks_cone)+10){
			error_cone=ticks_cone-abs(getMotorEncoder(R_lift));
			lifterror=error_cone;
			integral_cone=integral_cone+error_cone;
			if(error_cone==0||(abs(getMotorEncoder(R_lift))>ticks_cone)){
				integral_cone=0;
			}
			if(error_cone>5000){
				integral_cone=0;
			}
			derivative_cone=error_cone-prevError_cone;
			power_cone=error_cone;
			power_cone=error_cone*kP_cone+integral_cone*kI_cone+prevError_cone*kD_cone;
			liftpower=power_cone;
			motor[R_lift]=power_cone;
			wait1Msec(15);
		}
		motor[R_lift]=0;
	}
}


//   __   __        __      __   __        ___
//  / _` |__)  /\  |__)    /  ` /  \ |\ | |__
//  \__> |  \ /~~\ |__)    \__, \__/ | \| |___
//

void openClaw(){
	motor[claw] = 100;
	wait1Msec(100);
	motor[claw] = 0;
}

task grabCone(){
	if(floor1){
		armTarget=130;
		wait1Msec(500);
		height=0;
		wait1Msec(500);
		if(counter<4){
			height=2;
		}
		else{
			height=3;
		}
		wait1Msec(500);
		armTarget=0;
		openClaw();
		counter+=1;
		floor1=false;
	}
	if(preload){
		armTarget=130;
		wait1Msec(500);
		height=1;
		wait1Msec(500);
		if(counter<4){
			height=2;
			wait1Msec(500);
		}
		else{
			height=3;
			wait1Msec(750);
		}
		armTarget=0;
		openClaw();
		counter+=1;
		preload=false;
	}
}

//            ___  __
//   /\  |  |  |  /  \
//  /~~\ \__/  |  \__/
//

task autonomous()
{
	startTask(slewRate);
}


//        __   ___  __      __   __       ___  __   __
//  |  | /__` |__  |__)    /  ` /  \ |\ |  |  |__) /  \ |
//  \__/ .__/ |___ |  \    \__, \__/ | \|  |  |  \ \__/ |___
//

task usercontrol()
{
	int driveThreshold = 20;
	while (true){
		if(abs(vexRT[Ch3])>driveThreshold){
			motor[LEDrive] = vexRT[Ch3];
		}	else {
			motor[LEDrive] = 0;
		}
		if(abs(vexRT[Ch2])>driveThreshold){
			motor[REDrive] = vexRT[Ch2];
		}	else {
			motor[REDrive] = 0;
		}
		motor[clawArm] = (vexRT[Btn6UXmtr2]-vexRT[Btn5UXmtr2])*127;
		motor[claw] = vexRT[Btn7DXmtr2]*127;
		motor[fork] = (vexRT[Btn6U]-vexRT[Btn6D])*127;
		motor[L_lift] = (vexRT[Btn8DXmtr2]-vexRT[Btn8RXmtr2])*127;
	}
}