void grabBall(void)
{
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
//Open hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F3, !blink_on);
SetServo(handServo, 0.2f);
Wait(1.0);
lowerArm();
//Close hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F2, !blink_on);
SetServo(handServo, 0.65f);
Wait(1.0);
//arm goes up
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
raiseArm();
}
void positionArm(int position)
{// Positions arm to a specified place
if (getArmPotentiometer() < position )
{
while(getArmPotentiometer() < position && vexRT[Btn8R] == 0)
{
raiseArm();
driveWithJoysticks();
intakeControl();
}
stopArm();
}
else if (getArmPotentiometer() > position)
{
while (getArmPotentiometer() > position && vexRT[Btn8R] == 0)
{
lowerArm();
driveWithJoysticks();
intakeControl();
}
stopArm();
}
}
task main()
{
waitForStart();
init();
intakeOn(2);
raiseCube(4.5);
reverse(100,.7);
dropCube();
wait1Msec(1000);
cubeUp();
forward(100,.5);
right(100,1.5);
reverse(100,1);
left(100,1);
raiseArm(1);
reverse(100,.5);
left(100,1.5);
reverse(100,2);
allStop();
}
task main()
{
init();
while(nMotorEncoder[armR]<2000){
raiseArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[armR]);
}
}
task main()
{
waitForStart(); // Wait for the tele-op period to begin
while(true) // Infinite loop:
{
joyDrive();
raiseArm();
bucketM();
}
}
task main()
{
waitForStart();
forward(100,1);
init();
intakeOn(2);
left(100,.5);
raiseArm(1);
forward(100,1.8);
allStop();
}
void slolRof() {
setTank();
powerArm(0);
raiseArm(650);
powerArm(10);
forward(480);
wait1Msec(1000);
back(500);
basicMove(0, 0, 0);
wait1Msec(2000);
forward(470);
wait1Msec(1000);
back(450);
basicMove(0, 0, 0);
}
task main()
{
initializeRobot();
waitForStart();
raiseArm(ArmUpRotations);
intializeClaw();
int _dirAC = 0;
wait1Msec(200);
int sensorDirection = HTIRS2readDCDir(irSeeker);
if(sensorDirection < 5){
raiseArm(LeftArmUpRotations);
leftTurn(leftTurnRotations);
attackRack(leftFinalInches);
}
else{
firstMove(middleStartInches);
wait1Msec(300);
sensorDirection = HTIRS2readDCDir(irSeeker);
if(sensorDirection < 7){
leftTurn(middleTurnRotations);
attackRack(middleFinalInches);
}
else{
raiseArm(RightArmUpRotations);
firstMove(rightStartInches - middleStartInches);
leftTurn(middleTurnRotations);
attackRack(rightFinalInches);
}
}
retreat();
}
void forMuricaAndForAslan() {
setTank();
forward(1600);
setMorpheus();
basicMove(0, 10, -127);
wait1Msec(1000);
basicMove(0, 0, 0);
raiseArm(1200);
powerArm(10);
setTank();
forward(470);
basicMove(0, 0, 0);
outtakeT(1200);
back(600);
basicMove(0, 0, 0);
}
void forLols() {
setTank();
powerArm(0);
raiseArm(650);
powerArm(10);
motor[Intake1] = motor[Intake2] = -127;
forward(430);
wait1Msec(1000);
back(430);
basicMove(0, 0, 0);
//waitForButton();
wait1Msec(4000);
forward(480);
wait1Msec(1000);
back(500);
basicMove(0, 0, 0);
lowerArm(0);
motor[Intake1] = motor[Intake2] = 0;
basicMove(0, 0, 0);
}
void hangSide() {
motor(Intake1) =motor(Intake2) = 127;
forward(34);
wait1Msec(1500);
back(60);
basicMove(0, 0, 0);
wait1Msec(2000);
/*raiseArm(300);
outtakeT(2000);*/
motor(Intake1) = motor(Intake2) = 0;
powerArm(0);
basicMove(0, 0, 0);
raiseArm(600);
powerArm(20);
waitForButton();
outtakeT(3500);
basicMove(0, 0, 0);
powerArm(0);
basicMove(0, 0, 0);
}
void dropBall(void)
{
//move arm up halfway
SetPin(PIN_F2, blink_on);
SetPin(PIN_F3, !blink_on);
raiseArmPeak();
//open hand and wait for ball to drop
SetPin(PIN_F3, blink_on);
SetPin(PIN_F3, !blink_on);
SetServo(handServo, 0.2f);
Wait(3.0);
//close hand
SetPin(PIN_F3, blink_on);
SetPin(PIN_F2, !blink_on);
SetServo(handServo, 0.65f);
Wait(2.0);
//arm goes down
raiseArm();
}
// Most basic, scores preload, knocks over big balls.
void forGreaterJustice() {
setTank();
forward(1100);
raiseArm(1200);
powerArm(10);
//back(50);
basicMove(0, 0, 0);
motor(Intake1) =motor(Intake2) = -127;
wait1Msec(800);
back(400);
motor(Intake1) =motor(Intake2) = 0;
lowerArm(10);
back(600);
basicMove(0, 0, 0);
waitForButton();
// Push over big balls
//raiseArm(650);
powerArm(0);
//stopArm();
motor[Arm1] = motor[Arm2] = 0;
forLols();
}
task main()
{
//Initiate Robot
init();
//Raise Arm
while(nMotorEncoder[armR]<5){
raiseArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[armR]);
}
stop();
lsVal = LSvalNorm(LEGOLS);
while(lsVal>85){
lsVal = LSvalNorm(LEGOLS);
forward(10);
}
stop();
wait1Msec(500);
ClearTimer(T1);
while(time1[T1]<1000){
HTIRS2readAllDCStrength(HTIRS2,dcS1,dcS2,dcS3,dcS4,dcS5);
HTIRS2readAllACStrength(HTIRS2, acS1, acS2, acS3, acS4, acS5);
nxtDisplayCenteredTextLine(1,"IR:");
nxtDisplayCenteredTextLine(2,"%i",acS1);
nxtDisplayCenteredTextLine(3,"%i",acS2);
nxtDisplayCenteredTextLine(4,"%i",acS3);
nxtDisplayCenteredTextLine(5,"%i",acS4);
nxtDisplayCenteredTextLine(6,"%i",acS5);
}
if(acS2<20&&acS4<20&&acS3>20)
goto column2;
else if(acS2>acS4)
goto column3;
else
goto column1;
////////First Column/////////////
column1:;
stop();
nxtDisplayCenteredTextLine(1,"Column One:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);
////////Second Column/////////////
column2:;
lsVal = LSvalNorm(LEGOLS);
while(time1[T1]<2000){
if(lsVal<50){
forward(15);
}
else{
left(20);
}
lsVal = LSvalNorm(LEGOLS);
}
stop();
//forward(20);
//wait1Msec(1000);
//stop();
//forward(30);
//wait1Msec(250);
//stop();
while(true){nxtDisplayCenteredTextLine(1,"Column Two:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);}
////////Third Column//////////////
column3:;
stop();
while(true){nxtDisplayCenteredTextLine(1,"Column Three:");nxtDisplayCenteredTextLine(2,"\n%i\n%i\n%i",acS2,acS3,acS4);}
}
task main()
{
//while(true){
// nxtDisplayCenteredTextLine(1,"%i",SensorValue[lightSensor]);
//}
//waitForStart();
nMotorEncoder[weightedRingLight]=0;
int threshold=20;
int powerLevel=50;
int lines=0;
int prevColor=0;
int dcS1,dcS2,dcS3,dcS4,dcS5;
while(SensorValue[lightSensor]<threshold){
motor[motorFL] = -powerLevel;
motor[motorFR] = -powerLevel;
motor[motorBR] = -powerLevel;
motor[motorBL] = -powerLevel;
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
nxtDisplayCenteredTextLine(1,"IR%i",dcS3);
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=powerLevel;
motor[motorFR]=powerLevel;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=powerLevel;
motor[motorFR]=powerLevel;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=-powerLevel*3;
motor[motorFR]=-powerLevel*3;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=-powerLevel*2;
motor[motorFR]=-powerLevel*2;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
while(SensorValue[lightSensor]<threshold){
motor[motorFL]=-powerLevel;
motor[motorFR]=-powerLevel;
motor[motorBL]=0;
motor[motorBR]=0;
}
motor[motorFL]=0;
motor[motorFR]=0;
motor[motorBL]=0;
motor[motorBR]=0;
PlaySound(soundBeepBeep);
HTIRS2readAllDCStrength(HTIRS2, dcS1, dcS2, dcS3, dcS4, dcS5);
if(dcS3>10){
while(true){
raiseArm();
nxtDisplayCenteredTextLine(1,"IRBeacon Found");
}
}
wait1Msec(1000);
}
task usercontrol()
{
while(true)
{
driveWithJoysticks();
intakeControl();
if(vexRT[Btn6U]== 1 && vexRT[Btn6D] == 0 && vexRT[Btn8U]== 0 && vexRT[Btn8D]== 0)
{
raiseArm();
}
else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 1 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
{
lowerArm();
}
else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 1 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
{
positionArm(Arm_ScoreHeight);
}
else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 1 && vexRT[Btn8R] == 0)
{
positionArm(Arm_MinHeight);
}
else
{
stopArm();
}
if(vexRT[Btn7U] == 1)
{
if(btn7UPressed != 1)
{
currentIntakeSpeedStep++;
if(currentIntakeSpeedStep > 2)
{
currentIntakeSpeedStep = 2;
}
btn7DPressed = true;
}
}
else
{
btn7UPressed = false;
}
if(vexRT[Btn7D] == 1)
{
if(btn7DPressed != 1)
{
currentIntakeSpeedStep--;
if(currentIntakeSpeedStep < 1)
{
currentIntakeSpeedStep = 1;
}
btn7DPressed = true;
}
}
else
{
btn7DPressed = false;
}
switch(currentIntakeSpeedStep)
{
case 1:
currentIntakeSpeed = IntakeSlow;
break;
case 2:
currentIntakeSpeed = IntakeFast;
break;
default:
currentIntakeSpeed = IntakeFast;
break;
}
}
while ((vexRT[Btn7D] == 1) || (vexRT[Btn7U] == 1) || (vexRT[Btn7R] == 1) || (vexRT[Btn7L] == 1))
{
// Wait until button is released to go back to single joystick control.
driveWithJoysticks();
}
}
task main() {
waitForStart();
/*int raw = 0;
int nrm = 0;
bool active = true;
// Turn the light on
LSsetActive(LEGOLS);
nNxtButtonTask = -2;
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "LIGHT");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
nxtDisplayClearTextLine(7);
nxtDisplayTextLine(5, "Press [enter]");
nxtDisplayTextLine(6, "to toggle light");
wait1Msec(2000);*/
//while (true) {
// The enter button has been pressed, switch
// to the other mode
/*if (nNxtButtonPressed == kEnterButton) {
active = !active;
if (!active)
LSsetInactive(LEGOLS);
else
LSsetActive(LEGOLS);
// wait 500ms to debounce the switch
wait1Msec(500);
}
nxtDisplayClearTextLine(5);
nxtDisplayClearTextLine(6);
raw = LSvalRaw(LEGOLS);
nrm = LSvalNorm(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", raw);
nxtDisplayTextLine(6, "Norm: %4d", nrm);
wait1Msec(50);
}*/
int ser=0,fs1=0;
servo[servo1]=ser;
servo[servo2]=215-ser;
bool rightOfLine = true,touch=false;
LSsetActive(LEGOLS);
touch = TSreadState(LEGOTS);
lsVal = LSvalRaw(LEGOLS);
nMotorEncoder[arms] = 0; //Initiate Encoder Pos
wait1Msec(2500);
while(lsVal<360){
lsVal = LSvalRaw(LEGOLS);
forward(15);
}
stop();
//reverse(50);
wait1Msec(500);
stop();
wait1Msec(1000);
while(nMotorEncoder[arms]<30){
raiseArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
}stop();
wait1Msec(1000);
while(lsVal<210){
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
lsVal = LSvalRaw(LEGOLS);
left(50);
}
stop();
wait1Msec(1000);
fs1 = HTFreadSensor(HTFS1);
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
ClearTimer(T1);
while(time1[T1]<2500){
nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
fs1 = HTFreadSensor(HTFS1);
lsVal = LSvalRaw(LEGOLS);
touch = TSreadState(LEGOTS);
nxtDisplayTextLine(5, "Raw: %4d", lsVal);
if(lsVal>210){
while(time1[T1]<2500){
forward(20);
lsVal = LSvalRaw(LEGOLS);
nxtDisplayTextLine(5, "Raw: %4d", lsVal);
}
rightOfLine=!rightOfLine;
}
else if(lsVal<210){
if(rightOfLine){
rotateLeft(50);
}else{
rotateRight(50);
}
}
}
stop();
ser=100;
servo[servo1]=ser;
servo[servo2]=215-ser;
while(nMotorEncoder[arms]>5){
lowerArm(50);
nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
}stop();
reverse(50);
wait1Msec(2500);
}