void drawCircle(double radius)
{
double circumference = 2 * radius * PI;
double ratio = 1 + (1/radius);
double wheelTurns = circumference*(3.0/4.0);
driveRobot(wheelTurns, 60, ratio);
stopMotorsAndWait(0);
}
void drawSquare(double size)
{
int i;
for (i = 0; i < 4; i++)
{
driveRobot(size, 60, 1.0);
stopMotorsAndWait(1);
if (i < 3) {
turnRobot(90);
}
}
}
void drawStar(double size)
{
int i;
for (i = 0; i<5; i++)
{
driveRobot(size, 60, 1.0);
stopMotorsAndWait(1);
if (i < 4) {
turnRobot(144);
}
}
}
void drawTriangle(double size)
{
int i;
for (i = 0; i < 3; i++)
{
driveRobot(size, 60, 1.0);
stopMotorsAndWait(1);
if (i < 2) {
turnRobot(120);
}
}
}
task main() {
//waitForStart();
initializeRobot();
while (true) {
getJoystickSettings(joystick);
menu_main();
driveRobot();
// Activates Crate Uprighter, Use Button 5 on Controller to Activate
if (joy1Btn(5)) {
StartTask(runArm);
}
}
}
//int rPower=75, lPower=55;
task usercontrol()
{
while(1) {
motor[lDriveFront] = vexRT[Ch3];
motor[rDriveFront] = vexRT[Ch2];
motor[lDriveBack] = vexRT[Ch3];
motor[rDriveBack] = vexRT[Ch2];
if(testing == 3) //Tests auto
{
driveRobot(1,100,3685);
initializeTBHfast();
FwVelocitySet(lFly, 155, normalizeMotorPower(85));
FwVelocitySet(rFly, 155, normalizeMotorPower(85));
wait10Msec(7000)
motor[intakeRoller] = 50;
motor[intakeChain] = -50;
driveRobot(-1,100,20);
driveRobot(1,0,100000);
wait1Msec(5000);
}
if(vexRT[Btn6D] == 1)
{
motor[intakeRoller] = -127;
}
else if(vexRT[Btn6U] == 1 || testing == 1|| testing == 2)
{
motor[intakeRoller] = 127;
}
else
{
motor[intakeRoller] = 0;
}
if(vexRT[Btn5D] == 1 || testing == 1 || testing == 2)
{
motor[intakeChain] = -127;
}
else if(vexRT[Btn5U] == 1)
{
motor[intakeChain] = 127;
}
else
{
motor[intakeChain] = 0;
}
if(vexRT[Btn7U] == 1) //far shooting
{
initializeTBH();
FwVelocitySet(lFly, 146, normalizeMotorPower(85));
FwVelocitySet(rFly, 146, normalizeMotorPower(85));
}
else if(vexRT[Btn7D] == 1) //close shooting
{
initializeTBHClose();
FwVelocitySet(lFly, 94, normalizeMotorPower(55));
FwVelocitySet(rFly, 94, normalizeMotorPower(55));
}
else if(vexRT[Btn7R] == 1) //skills (purple) shooting
{
initializeTBHSkills();
FwVelocitySet(lFly, 105, normalizeMotorPower(70));
FwVelocitySet(rFly, 105, normalizeMotorPower(70));
}
else if(vexRT[Btn8D] == 1)
{
stopFlywheel();
}
writeDebugStreamLine("%d,%d,%d,%d,%d,%d,%d",nPgmTime, lFly.current, lFly.motor_drive, lFly.raw_last_rpm, rFly.current, rFly.motor_drive, rFly.raw_last_rpm);
wait1Msec(25);
}
}
