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()
{
initializeRobot();
//waitForStart();
moveDownWrist();
moveRobot();
liftArm();
wait1Msec(500);
rightTurnRobot();
wait1Msec(750);
shortMoveRobot();
wait1Msec(500);
moveUpWrist();
lowerArm();
}
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);
}
// 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 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);
}
