MarkIIIRewrite.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  flywheelEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  indexHigh,      sensorTouch)
#pragma config(Sensor, dgtl4,  indexLow,       sensorTouch)
#pragma config(Sensor, dgtl9,  upToSpeed,      sensorLEDtoVCC)
#pragma config(Sensor, dgtl10, encoderTest,    sensorTouch)
#pragma config(Sensor, dgtl11, tune,           sensorTouch)
#pragma config(Sensor, dgtl12, debug,          sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           rightWheel2,   tmotorVex393TurboSpeed_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           leftWheel13,   tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port3,           indexer,       tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           flywheel1,     tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           flywheel3,     tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           leftWheel2,    tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port7,           rightWheel13,  tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port8,           flywheel4,     tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port9,           flywheel2,     tmotorVex393TurboSpeed_MC29, openLoop)
#pragma config(Motor,  port10,          intake,        tmotorVex393HighSpeed_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(0)
#pragma userControlDuration(60)
#pragma systemFile            	// eliminates warning for "unreferenced" functions

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


bool debugMode = true;

//Stores the differient speeds for the velocity states of the robot
typedef struct {
	int velocity;
	int highSpeed;
	int lowSpeed;
	int ramp;
	int wait;
} flywheelShot;

flywheelShot longShot, midShot, pipeShot, holdShot;
flywheelShot currentShot;

void flywheelShots() {
	longShot.velocity = 800;
	longShot.highSpeed = 127;
	longShot.lowSpeed = 60;
	longShot.ramp = 58;
	longShot.wait = 580;

	midShot.velocity = 600;
	midShot.highSpeed = 127;
	midShot.lowSpeed = 45;
	midShot.ramp = 45;
	midShot.wait = 0;

	pipeShot.velocity = 570;
	pipeShot.highSpeed = 127;
	pipeShot.lowSpeed = 40;
	pipeShot.ramp = 45;
	pipeShot.wait = 0;

	holdShot.velocity = 300;
	holdShot.highSpeed = 90;
	holdShot.lowSpeed = 45;
	holdShot.ramp = 45;
	holdShot.wait = 0;
}

int flywheelVelocity;
int flywheelReverseStartThreshold = 10;

int intakeWaitTime;
int intakeMoveDownTime = 250;
int intakeShootVelocityThreshold = 150;
bool intakeAutonomousIntake;
bool intakeAutonomousIndexer;
bool intakeAutonomousShoot;

#include "JonLib/Drivebase.h"
#include "JonLib/PID.h"
#include "autonomous.c"

#warning "setLeftWheelSpeed"
void setLeftWheelSpeed ( int speed ) {
	motor[leftWheel13] = speed;
	motor[leftWheel2] = speed;
}

//Sets the speed of the wheels on the right side of the robot
#warning "setRightWheelSpeed"
void setRightWheelSpeed ( int speed ) {
	motor[rightWheel13] = speed;
	motor[rightWheel2] = speed;
}

//Logarithmic drivebase control
#warning "logDrive"
void logDrive () {
	setWheelSpeed(
	abs(vexRT(Ch3))*vexRT(Ch3)/127,
	(abs(vexRT(Ch2))*vexRT(Ch2)/127)>100?100:abs(vexRT(Ch2))*vexRT(Ch2)/127);
}

#warning "flywheelVelocityCalculation"
task flywheelVelocityCalculation() {
	long lastSysTime = nSysTime;
	while(true) {
		flywheelVelocity = (((float)-SensorValue[flywheelEncoder])/360)/(((float)(nSysTime-lastSysTime)==0?1:(float)(nSysTime-lastSysTime)/(float)60)/1000);
		SensorValue[flywheelEncoder] = 0;
		lastSysTime = nSysTime;
		delay(5);
	}
}

void flywheelLCD () {
	clearLCDLine(0);
	displayLCDNumber(0,1,flywheelVelocity);
	displayLCDNumber(0,5,currentShot.velocity);
	displayLCDNumber(0,10,motor[flywheel4]);
}

void flywheelLED() {
	if(motor[flywheel4]>90)
		SensorValue[upToSpeed] = 1;
	else
		SensorValue[upToSpeed] = 0;
}

void flywheelRampUp (int target) {
	while(motor[flywheel4] < target)
		motor[flywheel4]+=2;
}

#warning "flywheelControl"
task flywheelControl() {
	float kP = 0.1;

	motor[flywheel4] = 25;

	flywheelRampUp (currentShot.lowSpeed);

	int flywheelSpeedA, flywheelSpeedB;

	while(true) {

		flywheelSpeedA = currentShot.highSpeed + (currentShot.velocity-flywheelVelocity) * kP;
		flywheelSpeedB = currentShot.lowSpeed + (currentShot.velocity-flywheelVelocity) * kP;

		flywheelSpeedA = flywheelSpeedA>100?100:flywheelSpeedA;
		flywheelSpeedB = flywheelSpeedB>100?100:flywheelSpeedB;

		flywheelSpeedA = flywheelSpeedA<0?0:flywheelSpeedA;
		flywheelSpeedB = flywheelSpeedB<0?0:flywheelSpeedB;

		if(flywheelVelocity < currentShot.velocity+currentShot.ramp) {
			motor[flywheel4] = flywheelSpeedA;
		} else {
			motor[flywheel4] = flywheelSpeedB;
		}

		if(debugMode) {
			flywheelLCD();
			flywheelLED();
		}

		delay(30);
	}
}

#warning "startFlywheel"
void startFlywheel (flywheelShot shot) {
	currentShot.velocity = shot.velocity;
	currentShot.highSpeed = shot.highSpeed;
	currentShot.lowSpeed = shot.lowSpeed;
	currentShot.ramp = shot.ramp;
	currentShot.wait = shot.wait;
	if(flywheelVelocity >= 0)
		startTask(flywheelControl, kHighPriority);
	else
		motor[flywheel4] = 0;
}

void startFlywheel (int targetVelocity, int lowSpeed, int highSpeed, int rampThreshold, int waitTime = 0) {
	flywheelShot tempShot;
	tempShot.velocity = targetVelocity;
	tempShot.lowSpeed = lowSpeed;
	tempShot.highSpeed = highSpeed;
	tempShot.ramp = rampThreshold;
	tempShot.wait = waitTime;
	startFlywheel(tempShot);
}

#warning "stopFlywheel"
void stopFlywheel () {
	stopTask(flywheelControl);
	motor[flywheel4] = 0;
}

#warning "intakeControl"
task intakeControl () {
	while(true) {
		motor[intake] = ((vexRT(Btn5U)||intakeAutonomousIntake)-vexRT(Btn5D))*127;

		//Move ball from high limit switch to low limit switch
		if(vexRT(Btn6D) && SensorValue[indexHigh]) {
			motor[indexer] = -127;
			delay(intakeMoveDownTime);
		}

		//Shooting control
		if (vexRT(Btn6U) || intakeAutonomousShoot) {
			if(flywheelVelocity>intakeShootVelocityThreshold && time1[T1]>intakeWaitTime) {
				motor[indexer] = 127;
				while(SensorValue[indexHigh] && (vexRT(Btn6U)||intakeAutonomousShoot)) { delay(5); }
				clearTimer(T1);
			}
			else {
				motor[indexer] = (SensorValue[indexHigh])?0:127;
			}
		}

		//Move ball down even if there is a sensor we want
		else if (vexRT(Btn5D))
			motor[indexer] = -127;

		//Stop ball if ball is at a sensor
		else if(SensorValue[indexLow] && !SensorValue[indexHigh]) {
			motor[indexer] = 100;
			while(SensorValue[indexLow] && !SensorValue[indexHigh]) { delay(20); }
			if(SensorValue[indexHigh]) { motor[indexer] = 0; }
			wait1Msec(0);
			motor[indexer] = 0;
		}

		else
			motor[indexer] = 0;

		delay(30);
	}
}

void reverseFlywheel () {
	stopFlywheel();
	if(flywheelVelocity > flywheelReverseStartThreshold)
		motor[flywheel4] = -5;
	else if (flywheelVelocity <= flywheelReverseStartThreshold)
		motor[flywheel4] = -127;
}

#warning "init"
void init() {
	playTone(700,10);

	setBaudRate(UART1, baudRate57600);

	//Slave Motors
	slaveMotor(flywheel2,flywheel4);
	slaveMotor(flywheel3,flywheel4);
	slaveMotor(flywheel1,flywheel4);

	//Startup modes
	if(!debugMode)
		debugMode = (bool) SensorValue[debug];

	intakeAutonomousIndexer = false;
	intakeAutonomousIntake = false;
	intakeAutonomousIndexer = false;

	flywheelShots();

	startTask(intakeControl);
	startTask(flywheelVelocityCalculation);
}

void pre_auton()
{
  bStopTasksBetweenModes = true;
}

task autonomous()
{
	drivePID(1000);
}

task usercontrol() {

	init();

	while (true) {

	  logDrive();

	  if(vexRT(Btn7L))
	  	reverseFlywheel();

	 	if(vexRT(Btn8U)) {
			startFlywheel(pipeShot);
			while(vexRT(Btn8U)) { delay(10); }
		}

		else if(vexRT(Btn8R)) {
			startFlywheel(midShot);
			while(vexRT(Btn8R)) { delay(10); }
		}

		else if(vexRT(Btn8L)) {
			startFlywheel(longShot);
			while(vexRT(Btn8L)) { delay(10); }
		}

		else if(vexRT(Btn7D)) {
			//startFlywheel(holdShot);
			startFlywheel(200, 30, 60, 10, 0);
			while(vexRT(Btn7D)) { delay(10); }
		}

		else if(vexRT(Btn7U)) {
			motor[flywheel4] = 60;
		}

		else if(vexRT(Btn8D))
			stopFlywheel();

	}
}