void init() {
playTone(700,10);
startTask(LCD);
startTask(flywheelVelocity);
setBaudRate(UART1, baudRate57600);
//Slave Motors
slaveMotor(flywheel2,flywheel4);
slaveMotor(flywheel3,flywheel4);
slaveMotor(flywheel1,flywheel4);
//Startup modes
if(!debugMode)
debugMode = (bool) SensorValue[debug];
if(!tuneMode)
tuneMode = (bool) SensorValue[tune];
if(!encoderTestMode)
encoderTestMode = (bool) SensorValue[encoderTest];
//Boot into test encoder mode
if(encoderTestMode)
testEncoder();
bool autonIntake = false;
bool autonIndex = false;
bool autonShoot = false;
}
void initialize(){
slaveMotor(backRight, frontRight);
slaveMotor(backLeft, frontLeft);
slaveMotor(liftRight, liftLeft);
resetMotorEncoder(liftLeft);
resetMotorEncoder(backLeft);
resetMotorEncoder(claw);
wait1Msec(100);
}
task autonomous()
{
// ..........................................................................
// Insert user code here.
// ..........................................................................
slaveMotor(gb2,gb1);
slaveMotor(gb3, gb2);
slaveMotor(gb4, gb3);
bool a = true;
clearTimer(T1);
while(a)
{
if(time1(T1)<3000)
cat(-127);
if(time1(T1)>60000)
a=false
}
// Remove this function call once you have "real" code.
AutonomousCodePlaceholderForTesting();
}
////////////////////////////////////////
//autonomous
task autonomous()
{
slaveMotor(armL2, armL3);
slaveMotor(armL1, armL2);
slaveMotor(armR1, armR3);
slaveMotor(armR2, armR1);
resetIME();
grabAuto(127, 1250);//clutch preload
resetIME();
drive(127, 1000); //drive forward
resetIME();
rotateL(127, 1000);//turn 180 degrees
resetIME();
drive(-127, -850);//reverse to wall
resetIME();
armAuto(127, 250);//lift arm
resetIME();
grabAuto(-127, 1500);//release preload
resetIME();
armAuto(-127, -800);//arm back down
resetIME();
rotateR(127, 400);//rotate towards cube
resetIME();
drive(127,500);//drive towards cube
resetIME();
grabAuto(127, 1250);//clutch cube
resetIME();
rotateL(127, 400);//rotate towards fence
resetIME();
drive(-127, -850);//backup
resetIME();
armAuto(127,250);//raise arm
resetIME();
grabAuto(-127, 1250);//dump cube
resetIME();
armAuto(-127, -800);//lower arm
resetIME();
}
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);
}
task usercontrol()
{
// User control code here, inside the loop
slaveMotor(gb2,gb1);
slaveMotor(gb3, gb2);
slaveMotor(gb4, gb3);
while (true)
{
/*motor[frontLeft] = vexRT[Ch1] + vexRT[Ch2];//x drive arcade
motor[rearRight] = vexRT[Ch1] - vexRT[Ch2];
motor[rearLeft] = vexRT[Ch4] + vexRT[Ch3];
motor[frontRight] = vexRT[Ch4] - vexRT[Ch3];*/
/*motor[frontLeft] =vexRT[Ch3];//tank drive arcade
motor[rearLeft]=vexRT[Ch3];
motor[frontRight]= -vexRT[Ch2];
motor[rearRight]=-vexRT[Ch2];*/
driveL3ft(Ch3);
driveR1ght(Ch2);
if (vexRT[Btn6U]==1)
{
cat(-127);//up
}
else if(vexRT[Btn6D] == 1)
{
cat(127);//down
}
//for catapult with high tension
/*else if(vexRT[Btn5D]==1)
{
cat(10);//hold
}*/
else
{
cat(0);//no power
}
}
}
//Task controlling behavior during Autonomous period
task usercontrol()
{
mode = "Driver";
//Start necessary tasks for user control
startTask(Drive);
startTask(Intaking);
startTask(AutoLoading);
startTask(Launch);
startTask(Aim);
startTask(SoundEffects);
startTask(EmergencyOverride);
startTask(SFXOverride);
startTask(LCD);
slaveMotor(Out2, Out1);
while(true) //Keep this task going so that the Vex Competition system does not mistake the robot for disconnected
{
EndTimeSlice();
}
}
task usercontrol()
{
slaveMotor(shooterLB, shooterL);
slaveMotor(shooterRB, shooterR);
while(true)
{
//Warning Killer
if (0 == 1)
{
warningKiller();
rotateL(0);
rotateR(0);
}
//All the ints
int intakeForward = vexRT[Btn6U];
int intakeBackwards = vexRT[Btn5U];
int deadBand = 10;
//All the floats
float xL = vexRT[Ch4];
float yL = vexRT[Ch3];
float xR = vexRT[Ch1];
float yR = vexRT[Ch2];
//Intake & Shooter
if (vexRT[Btn7U])
{
motor[shooterL] = 100;//These numbers are abritrary. i.e. 100 = fast, 10 = slow, 0 = no power
motor[shooterR] = 100;
}
else
{
motor[shooterL] = 0;
motor[shooterR] = 0;
}
setIntake(intakeForward*75, intakeBackwards*75);
//Drive Code
motor[leftFront] = xR + yR;
motor[rightRear] = xR - yR;
motor[leftRear] = xL + yL;
motor[rightFront] = xL - yL ;
//Deadband
if(xL > deadBand || xL < -deadBand)
{
xL = xL;
}
else
{
xL = 0;
}
if(yL > deadBand || yL < -deadBand)
{
yL = yL;
}
else
{
yL = 0;
}
if(xR > deadBand || xR < -deadBand)
{
xR = xR;
}
else
{
xR = 0;
}
if(yR > deadBand || yR < -deadBand)
{
yR = yR;
}
else
{
yR = 0;
}
}
}
