2016CWS6.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           lmWheel,       tmotorVex393TurboSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           lbWheel,       tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           rbWheel,       tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           rmWheel,       tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           rfWheel,       tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          lfWheel,       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!
int velocity;
float flywheelSetpoint;
float motorSpeed;
float flywheelSpeed;
float flywheelKp = .01;
int position1;
int position2;


/*task flywheelVelocity(){
	while(true){
		position1 = nMotorEncoder(lrFlywheel);
		wait1Msec(200);
		position2 = nMotorEncoder(lrFlywheel);
		wait1Msec(200);
		velocity = (position2 - position1)/4;
	}
}

task flywheelP(){
	float error;

	startTask(flywheelVelocity);

	while(true){

		error = flywheelSetpoint - velocity;
		flywheelSpeed += (flywheelKp*error);

		if(flywheelSpeed > 127)
			flywheelSpeed = 127;
		else if (flywheelSpeed < 0)
			flywheelSpeed = 0;

		motor[urFlywheel] = flywheelSpeed;
		motor[lrFlywheel] = flywheelSpeed;
		motor[ulFlywheel] = flywheelSpeed;
		motor[llFlywheel] = flywheelSpeed;
		wait1Msec(30);
	}
}
*/
task drive(){
	while(true){
		motor[rfWheel] = vexRT(Ch2);
		motor[rmWheel] = vexRT(Ch2);
		motor[rbWheel] = vexRT(Ch2);
		motor[lfWheel] = vexRT(Ch3);
		motor[lmWheel] = vexRT(Ch3);
		motor[lbWheel] = vexRT(Ch3);


		wait1Msec(30);
	}
}
/*


void speedUpFlywheel(float setpoint){
	if(motorSpeed < setpoint)
		motorSpeed += 1;
	else
		motorSpeed = setpoint;

	flywheelSetpoint = motorSpeed;

	wait1Msec(150);
}

void slowDownFlywheel(){
if(motorSpeed > flywheelSetpoint)
motorSpeed -= 2;
else
motorSpeed = flywheelSetpoint;

motor[urFlywheel] = motorSpeed;
motor[lrFlywheel] = motorSpeed;
motor[ulFlywheel] = motorSpeed;
motor[llFlywheel] = motorSpeed;

wait1Msec(150);
}

task feeder(){
	while(true){
		if(vexRT(Btn5U)){
			if(velocity>flywheelSetpoint-7)
				motor[intake2] = 127;
			else
				motor[intake2] = 0;
		}
		else if(vexRT(Btn5D))
			motor[intake2] = -127;
		else
			motor[intake2] = 0;

	}
	wait1Msec(30);
}

task shooter(){
	while(true){
		if(vexRT(Btn6U)){
			while(!vexRT(Btn6D))
				flywheelSetpoint = 20;
			//flywheelSpeed = 90;
		}
		else if(vexRT(Btn6D)){
			while(!vexRT(Btn6U)){}
			//slowDownFlywheel();

		}
		wait1Msec(30);
	}
}


*/
void pre_auton()
{
	bStopTasksBetweenModes = true;
}


task autonomous()
{
	AutonomousCodePlaceholderForTesting();  // Remove this function call once you have "real" code.
}

task usercontrol()
{
	//startTask(shooter);
	//startTask(flywheelP);
	//startTask(feeder);
	startTask(drive);
	while (true)
	{
	}

}