2016JPD34_QM3.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl11, slowDown,       sensorTouch)
#pragma config(Sensor, dgtl12, ballHigh,       sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           feeder,        tmotorVex393TurboSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           LUflywheel,    tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           LDflywheel,    tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           LBMdrive,      tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           LFdrive,       tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           RBMdrive,      tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           RFdrive,       tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port8,           RUflywheel,    tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port9,           RDflywheel,    tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port10,          intake1,       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!

void
FwMotorSet( int value )
{
	motor[ LDflywheel ] = value;
	motor[ LUflywheel ] = value;
	motor[ RDflywheel ] = value;
	motor[ RUflywheel ] = value;
}

task drive(){
	//Drive loop
	while(true){
		motor[LFdrive] 	= vexRT(Ch2);
		motor[LBMdrive]	= vexRT(Ch2);
		motor[RFdrive] 	= vexRT(Ch3);
		motor[RBMdrive]	= vexRT(Ch3);
		wait1Msec(25);
	}
}

int feederWaitTime = 2000;
bool autoFeeder = false;
task feederWait () {
	wait1Msec(1500); //Wait time to delay for auto feeder
	clearTimer(T4);
	while(true) {
		if(!SensorValue[ballHigh]) {
			motor[feeder] = 127;
			} else if(time1[T4]>=feederWaitTime) {
			motor[feeder] = 127;
			clearTimer(T4);
			wait1Msec(200);
			} else {
			motor[feeder] = 0;
		}
		wait1Msec(25);
	}
}

int speed = 80;
bool fastMode = true;
task shooterDJ () {
	startTask(feederWait);
	int timesFed = 0;
	bool canRunAgain = true;
	while (true)
	{
		if(vexRT(Btn7U))
			speed++;
		if(vexRT(Btn7D))
			speed--;
		if(SensorValue[ballHigh]&&canRunAgain) {
			timesFed++;
			speed+=22;//7;
		}
		if(timesFed>0 & canRunAgain)
			canRunAgain = false;
		FwMotorSet(speed);
		//sprintf( str, "%4d %4d", target_velocity,  motor_velocity, nImmediateBatteryLevel/1000.0 );
		wait1Msec(200);
	}
}

task shooter () {

	bool canRunAgain = true;
	while (true)
	{
		if(vexRT(Btn7U))
			speed++;
		if(vexRT(Btn7D))
			speed--;

		FwMotorSet(speed);
		//sprintf( str, "%4d %4d", target_velocity,  motor_velocity, nImmediateBatteryLevel/1000.0 );
		wait1Msec(200);
	}
}

void shooterPowerDown () {
	stopTask(feederWait);
	stopTask(shooter);
	stopTask(shooterDJ);
	motor[feeder]=0;
	int motorSpeed = motor[LUflywheel];
	while(motorSpeed>0){
		motorSpeed-=2;
		FwMotorSet(motorSpeed);
		wait1Msec(50);
	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task driveForwardEndAutonomous () {
	int driveSpeed = -127;
	motor[LFdrive] 	= driveSpeed;
	motor[LBMdrive]	= driveSpeed;
	motor[RFdrive] 	= driveSpeed;
	motor[RBMdrive]	= driveSpeed;
	wait1Msec(1100);
	driveSpeed = 0;
	motor[LFdrive] 	= driveSpeed;
	motor[LBMdrive]	= driveSpeed;
	motor[RFdrive] 	= driveSpeed;
	motor[RBMdrive]	= driveSpeed;
	stopTask(driveForwardEndAutonomous);
}

task autonomous()
{
	clearTimer(T2);
	speed = 105; //was 97 into QM3-2
	feederWaitTime = 900;
	startTask(shooterDJ);
	motor[intake1] = 127;
	while(time1[T2]<9300) {wait1Msec(25);}

	motor[intake1] = 0;
	startTask(driveForwardEndAutonomous);
	shooterPowerDown();
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
bool autoStartShooter = false; //for testing so we don't have to smack the controller each and every time

task usercontrol() {
	startTask(drive);
	while(true) {
		if(vexRT(Btn8U)) {
			shooterPowerDown();
			autoFeeder = false;
		}
		if(vexRT(Btn8D) || autoStartShooter) {
			autoFeeder = true;
			if(fastMode) {
				speed = 77;
				feederWaitTime = 950;
				} else {
				speed = 72;
				feederWaitTime = 1300;
			}
			startTask(shooterDJ);
		}
		if(vexRT(Btn5D))
			motor[feeder] = 127;
		else if(!autoFeeder)
			motor[feeder] = 0;
		if(vexRT(Btn5U))
			motor[intake1] = 127;
		else if(!vexRT(Btn5U) && !vexRT(Btn7R))
			motor[intake1] = 0;
		if(vexRT(Btn6D)) {
			speed = 55;
			startTask(shooter);
		}
		else if(vexRT(Btn6U)){
			speed = 75;
			startTask(shooter);
		}
		if(vexRT(Btn7R)){
			motor[intake1] = -127;
			motor[feeder] = -127;
		}


		wait1Msec(25);
	}
}