BangBang.c

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

//Enum for differient flywheel velocities
typedef enum { FW_OFF = 0, FW_IDLE = 60, FW_ON = 20, FW_BANG = 127 } flywheelStates;

#warning "updateFlywheel"
int currentFlywheelState = FW_OFF;
int targetFlywheelState = FW_OFF;
int flywheelBangTime = 1000; //ms

task updateFlywheelBang () {
	if(abs(targetFlywheelState-motor[flywheel1]) > 5) {//threshold to set currentFlywheelState
		currentFlywheelState = targetFlywheelState;
	} else if(targetFlywheelState == FW_BANG && currentFlywheelState == FW_ON) {
		motor[flywheel1] = FW_BANG;
		wait1Msec(flywheelBangTime);
		motor[flywheel1] = FW_ON; //can also sub with updateFlywheel(FW_ON); as well
	}
	while (motor[flywheel1] < targetFlywheelState) {
		if(targetFlywheelState-motor[flywheel1] == 1)
			motor[flywheel1]+=1;
		else
			motor[flywheel1]+=2;
	}
	while (motor[flywheel1] > targetFlywheelState) {
		if(motor[flywheel1]-targetFlywheelState == 1)
			motor[flywheel1]-=1;
		else
			motor[flywheel1]-=2;
	}
}

void init() {
	slaveMotor(flywheel2,flywheel4);
	slaveMotor(flywheel3,flywheel4);
	slaveMotor(flywheel1,flywheel4);
	startTask(updateFlywheel);
}

task main () {
	init();
	motor[flywheel4] = FW_ON;
	while(true) {
		//if(vexRT(Btn7U))
		//	targetFlywheelState = FW_IDLE;
		//else if(vexRT(Btn7D))
		//	targetFlywheelState = FW_OFF;
		//else if(vexRT(Btn7L))
		//	targetFlywheelState = FW_ON;
		//else if(SensorValue[outtake])
		//	targetFlywheelState = FW_BANG;
	if(getMotorVelocity(flywheel4)<150)
		motor[flywheel4] = FW_BANG;
	else
		motor[flywheel4] = FW_ON;

	}
}