2016CWS3.c

#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    ,               sensorAnalog)
#pragma config(Sensor, dgtl1,  shooterL,       sensorTouch)
#pragma config(Sensor, dgtl2,  topIntakeL,     sensorTouch)
#pragma config(Sensor, dgtl3,  tiltL,          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(Motor,  port1,           LMbase,        tmotorVex393TurboSpeed_HBridge, openLoop, encoderPort, I2C_3)
#pragma config(Motor,  port2,           LFbase,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           RBbase,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           tilt,          tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port5,           RMbase,        tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port6,           RFbase,        tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           rShooter,      tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_4)
#pragma config(Motor,  port8,           lShooter,      tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           rIntake,       tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port10,          LBbase,        tmotorVex393TurboSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

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

float shooterKp = 0.3;
float tiltKp = 0.5;
int shooterSetpoint;
int tiltSetpoint;

task drive(){
	while(true){
		motor[RFbase] = -vexRT[Ch2];
		motor[RMbase] = -vexRT[Ch2];
		motor[RBbase] = -vexRT[Ch2];

		motor[LFbase] = vexRT[Ch3];
		motor[LMbase] = vexRT[Ch3];
		motor[LBbase] = vexRT[Ch3];
		wait1Msec(25);
	}
}

bool isBallInShooter(){
	return((bool)(SensorValue(shooterL)));
}

bool isBallInIntake(){
	return((bool)SensorValue(topIntakeL));
}

task intake(){
	while(true){
		if(!isBallInShooter()){
			if(vexRT[Btn5U])
				motor[rIntake] = 127;
			else if(vexRT[Btn5D])
				motor[rIntake] = -127;
			else
				motor[rIntake] = 0;
		}
		else if(isBallInIntake()){
			if(vexRT[Btn5D])
				motor[rIntake] = -127;
			else
				motor[rIntake] = 0;
		}
		else{
			if(vexRT[Btn5U])
				motor[rIntake] = 127;
			else if(vexRT[Btn5D])
				motor[rIntake] = -127;
			else
				motor[rIntake] = 0;
		}
		wait1Msec(25);
	}
}

task shooterP(){

	float encoderValue;
	float error;
	float drive;

	while(true){
		encoderValue = nMotorEncoder(rShooter);
		error = shooterSetpoint - encoderValue;
		drive = (shooterKp*error);
		if(drive > 127)
			drive = 127;

		motor[rShooter] = drive;
		motor[lShooter] = drive;

		wait1Msec(25);
	}
}

task tiltP(){

	float encoderValue;
	float error;
	float drive;

	while(true){
		encoderValue = SensorValue(I2C_1);
		error = tiltSetpoint - encoderValue;
		drive = (tiltKp*error);
		if(drive > 127)
			drive = 127;
		else if(drive < -127)
			drive = -127;

		motor[tilt] = drive;

		wait1Msec(25);
	}
}

task tilting(){
	startTask(tiltP);
	while(true){
		if(tiltL)
			nMotorEncoder(tilt) = 0;
		if(vexRT(Btn8D))
			tiltSetpoint = 0;
		else if(vexRT(Btn8L))
			tiltSetpoint = -1500;
		else if(vexRT(Btn8U))
			tiltSetpoint = -2800;
		wait1Msec(25);
	}
}

void shootCycle(){
	shooterSetpoint += 1175;
}

task shooter(){
	startTask(shooterP);
	while(true){
		if(isBallInShooter() && vexRT[Btn6U]){
			shootCycle();
			while(isBallInShooter() && vexRT[Btn6U])
				wait1Msec(25);
		}
		wait1Msec(300);
	}
}

void pre_auton()
{
	shooterSetpoint = 0;
	while(!SensorValue(tiltL))
		motor[tilt] = 127;
	motor[tilt] = -10;
	SensorValue(I2C_1) = 0;
	nMotorEncoder(tilt) = 0;
	wait1Msec(2000);
	motor[tilt] = 0;
}


task autonomous()
{
}

task shooterTest(){
	while(true){
		if(vexRT[Btn6U]){
		//if(nMotorEncoder(rShooter) < 1175){
				motor[rShooter] = 127;
				motor[lShooter] = 127;
			//}
			//else{
				//motor[rShooter] = 15;
				//motor[lShooter] = 15;
			//}
		}
		else{
			motor[rShooter] = 0;
			motor[lShooter] = 0;
		}
		wait1Msec(15);
	}
}

task usercontrol()
{
	startTask(drive);
	//startTask(shooter);
	startTask(shooterTest);
	startTask(tilting);
	startTask(intake);
}