bool innerTS(string side="both")
{
if (side == "both")
{
if (TSreadState(msensor_S2_3)) return true;
else if (TSreadState(msensor_S2_4)) return true;
else return false;
}
else if (side == "left") return TSreadState(msensor_S2_4);
else if (side == "right")return TSreadState(msensor_S2_3);
else return false;
}
void PressBumperToContinue()
{
nxtScrollText("");
nxtScrollText("press button");
nxtScrollText("to continue");
PlaySoundFile("! Attention.rso");
while (!TSreadState(BUMPER))
wait1Msec(10); // not pressed
while (TSreadState(BUMPER))
wait1Msec(10); // pressed
wait1Msec(500);
}
task main() {
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
HTSMUXscanPorts(HTSMUX);
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "TOUCH");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
while (true) {
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
// Check if the sensor is pressed or not.
if (TSreadState(msensor_S1_1))
nxtDisplayCenteredBigTextLine(3, "ACTIVE");
else
nxtDisplayCenteredBigTextLine(3, "INACTIVE");
wait1Msec(50);
}
}
void FollowSegment()
{
int steering;
T("enter FollowSegment");
ClearTimer(T1);
long lastTime = time1[T1];
while (true) {
int timeNow = time1[T1];
int elapsedTime = timeNow - lastTime;
lastTime = timeNow;
int llAvg = LLreadAverage(LINELEADER);
unsigned byte llResult = LLreadResult(LINELEADER);
if (llAvg != 0)
{
steering = DoPID(llAvg, 45, Kp, Ki, Kd, elapsedTime); // 45 is our goal, set point
//steering = llAvg;
motor[LEFT] = clip((basePower + steering), Min, Max);
motor[RIGHT] = clip((basePower - steering), Min, Max);
// data logging
int heading = HTMCreadHeading(COMPASS);
WriteShort(hFileHandle, nIoResult, timeNow);
WriteShort(hFileHandle, nIoResult, llAvg);
WriteShort(hFileHandle, nIoResult, steering);
WriteFloat(hFileHandle, nIoResult, integral);
WriteFloat(hFileHandle, nIoResult, derivative);
WriteShort(hFileHandle, nIoResult, elapsedTime);
WriteShort(hFileHandle, nIoResult, heading);
WriteByte(hFileHandle, nIoResult, llResult);
}
else
{
// stuff gone wrong, we lost the line
// this may be where we check result for an intersection
PlaySound(soundException);
}
if (TSreadState(BUMPER))
break;
lastTime = timeNow; // using the variable, not the actual clock
//wait1Msec(10); // some throttling
}
T("leave FollowSegment");
}
task main()
{
int mouthdown=214;
disableDiagnosticsDisplay();
eraseDisplay();
nxtDisplayCenteredBigTextLine(1,"Lift");
nxtDisplayCenteredBigTextLine(3,"Utility");
nxtDisplayCenteredBigTextLine(6,"Down Up");
//nxtDisplayCenteredBigTextLine(3," LEFT=DOWN");
while(true)
{
//nxtDisplayCenteredTextLine(5,"%d", nNxtButtonPressed);
if(nNxtButtonPressed==1)
{
servo[mouth]=mouthdown;
motor[elevator1]= 90;//Raises the elevator
motor[elevator2]= 90;
}
// lower elevator
else if(nNxtButtonPressed==2 && TSreadState(((tMUXSensor)touch))==0)
{
servo[mouth]=mouthdown;
motor[elevator1]= -90;
motor[elevator2]= -90;
}
else
{
motor[elevator1]=0;
motor[elevator2]=0;
}
if(nNxtButtonPressed==3)
{//middle button
nMotorEncoder[elevator1]=0;
nMotorEncoder[elevator2]=0;
}
nxtDisplayCenteredTextLine(5,"%d , %d", nMotorEncoder[elevator1], nMotorEncoder[elevator2]);
}
}
task main()
{
while (true)
{
// Check if the sensor is pressed or not.
if (TSreadState(LEGOTOUCH))
{
motor(rightwheel) = 20;
motor(leftwheel) = 20;
}
}
}
task main() {
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "TOUCH");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
while (true) {
if (TSreadState(LEGOTS))
nxtDisplayCenteredBigTextLine(3, "ACTIVE");
else
nxtDisplayCenteredBigTextLine(3, "INACTIVE");
wait1Msec(50);
}
}
void initializeRobot()
{
LSsetActive(lineFollower);
setWrist (.95);
while (!TSreadState (liftDownSensor))
motor [linearSlides]=-100;
motor [linearSlides]=0;
servo [rampLatch] = rampLatchClosed;
servo [seekerPivot]= seekerUp;
HTIRS2setDSPMode(irSeeker, DSP_1200);
wait1Msec (500);
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
return;
}
task main() {
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "TOUCH");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
while (true) {
// Check if the sensor is pressed or not.
if (TSreadState(LEGOTOUCH))
nxtDisplayCenteredBigTextLine(3, "ACTIVE");
else
nxtDisplayCenteredBigTextLine(3, "INACTIVE");
wait1Msec(50);
}
}
void score()
{
motor[lift] = 75;
bool raising = false;
int timeValue = 7450;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (externalBattery == -1)
writeDebugStreamLine("%d", time1[T3]);
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
servo[output] = 244;
}
task Arm()
{
int count = 0;
float lightVal = 0;
float lightAvg = 0;
bool bucketFull = false;
bool counted = false;
while(true)
{
getJoystickSettings(joystick);
if (joy1Btn(BTN_LB) && !joy1Btn(BTN_LT))
motor[lift] = 75;
else if (joy1Btn(BTN_LT) && !joy1Btn(BTN_LB))
motor[lift] = -75;
else
motor[lift] = 0;
if (joy1Btn(BTN_RB) && !joy1Btn(BTN_RT) && runIntake)
motor[intake] = 100;
else if (joy1Btn(BTN_RT) && !joy1Btn(BTN_RB) && runIntake)
motor[intake] = -100;
else
motor[intake] = 0;
lightVal = HTSPBreadADC(htspb, 0, 10);
//writeDebugStreamLine("Count: %d", count);
//writeDebugStreamLine("Avg: %d", lightAvg);
//writeDebugStreamLine("Val: %d", lightVal);
if (lightVal > (lightAvg - 40) && !counted && time1[T3] > 1200)
{
count += 1;
counted = true;
writeDebugStreamLine("Count: %d", count);
//writeDebugStreamLine("Avg: %d", lightAvg);
//writeDebugStreamLine("Val: %d", lightVal);
ClearTimer(T3);
}
else if (counted && lightAvg < (lightVal + 15))
{
counted = false;
}
else if (time1[T2] > 500)
{
lightAvg = lightVal;
ClearTimer(T2);
}
if (count == 5)
{
//bucketFull = true;
ClearTimer(T1);
count = 0;
}
if (time10[T1] > 300 && bucketFull)
{
runIntake = false;
bucketFull = false;
}
if (TSreadState(touch) == 1 && !closeOverride)
servo[inputstop] = 130;
else if (TSreadState(touch) == 0 && !closeOverride)
servo[inputstop] = 235;
if (time1[T4] > 5000)
closeOverride = false;
}
}
task main(){
calibrate();
while(true){
length = USreadDist(USBack);
rightWidth = USreadDist(USRight);
leftWidth = USreadDist(USLeft);
haveBall = TSreadState(HaveBaller);
frontIRValue = HTIRS2readACDir(IRFront);
currentRelCompass = HTMCreadRelativeHeading(Compass);
HTCS2readRGB(Colour, currentRed, currentGreen, currentBlue);
isWhite = (currentRed > whiteThreshold && currentGreen > whiteThreshold && currentBlue > whiteThreshold);
nxtDisplayTextLine(1, "Com: %4d", currentRelCompass);
nxtDisplayTextLine(2, "IR: %4d", frontIRValue);
nxtDisplayTextLine(3, "Len: %4d", length);
nxtDisplayTextLine(4, "RWid: %4d", rightWidth);
nxtDisplayTextLine(5, "LWid: %4d", leftWidth);
nxtDisplayTextLine(6, "Ball: %4d", haveBall);
if(isWhite){
nxtDisplayTextLine(7, "White: Yes");
}
else{
nxtDisplayTextLine(7, "White: No");
}
if(currentRelCompass < 0 - nbound){
move(CW, turnPower);
}
else if(currentRelCompass > 0 + nbound){
move(CC, turnPower);
}
else{
switch(frontIRValue){
case 0:
move(ST);
break;
case 1:
move(L);
break;
case 2:
move(L);
break;
case 3:
move(L);
break;
case 4:
move(L);
break;
case 5:
if(leftWidth < 45 && leftWidth != 0){
if(length < 20){
move(F);
}
else{
move(R);
}
}
else if(rightWidth < 45 && rightWidth != 0){
if(length < 20){
move(F);
}
else{
move(L);
}
}
else{
if (length > fbound){
move(B);
}
else if(length < 8){
move(F);
}
else{
move(ST);
}
}
break;
case 6:
move(R);
break;
case 7:
move(R);
break;
case 8:
move(R);
break;
case 9:
move(R);
break;
}
wait1Msec(50);
}
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
//Pos 1: 0 -> 0
//Pos 2: 0 -> 5
//Pos 3: 5
Move(-23, 0.6);
wait10Msec(100);
int irPos = 0;
writeDebugStreamLine("%d", HTIRS2readDCDir(ir));
if (HTIRS2readDCDir(ir) == 0)
{
writeDebugStreamLine("%d", HTIRS2readDCDir(ir));
if (HTIRS2readDCDir(ir) == 0)
irPos = 1;
else
irPos = 2;
}
else
irPos = 3;
writeDebugStreamLine("%d", irPos);
if (irPos == 3)
{
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
getUltraAngle(false);
Move(-15, 0.5);
Move(-(((float)USreadDist(ultraSonic) / 2.54) - 9), 0.2);
wait10Msec(100);
//Move(-34, 0.6);
wait10Msec(1);
score();
}
else if (irPos == 2)
{
Turn(-60);
wait10Msec(1);
Move(-26, 0.4);
wait10Msec(1);
Turn(104);
wait10Msec(1);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
wait10Msec(1);
Turn(90 - getUltraAngle(false));
wait10Msec(1);
Move(-(((float)USreadDist(ultraSonic) / 2.54) - 9), 0.2);
wait10Msec(1);
score();
}
else
{
//leave zone
Move(13, 1);
wait10Msec(10);
Turn(28);
wait10Msec(10);
Move(-65, 1);
wait10Msec(10);
Turn(-23);
wait10Msec(10);
Move(-37, 0.9);
wait10Msec(10);
Move(-10, 0.2);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//while(true){}
float Angle = 0;
Angle = getUltraAngle(true);
/*if (Angle == -1)
Move(-5, 0.3);
else if (Angle >= 86 && Angle <= 90)
break;*/
writeDebugStreamLine("%f", Angle);
Turn(Angle - 90);
float dist = -10;
Move(dist, 0.25);
//grab goal
wait10Msec(30);
servo[hook1] = 235;
servo[hook2] = 30;
wait10Msec(150);
Turn(90 - Angle);
wait10Msec(10);
Move(117, 1);
wait10Msec(10);
Turn(-100);
wait10Msec(10);
Move(-25, 0.5);
wait10Msec(10);
//score
motor[lift] = 75;
raising = true;
timeValue = 800;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//Move(3, 0.2);
wait10Msec(10);
servo[output] = 250;
wait10Msec(500);
servo[output] = 130;
//release goal
servo[hook1] = 0;
servo[hook2] = 255;
wait10Msec(300);
Move(5, 1);
wait10Msec(30);
Turn(90);
}
if (irPos != 1)
{
//KickStand
wait10Msec(200);
servo[output] = 130;
motor[lift] = -75;
bool raising = false;
int timeValue = 4000;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
Turn(90);
wait10Msec(1);
Move(-10, 0.5);
wait10Msec(1);
Turn(80);
wait10Msec(1);
Move(20, 0.9);
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
//Off ramp
Move(-75, 0.6);
wait10Msec(10);
//Move(-20, 0.25);
//wait10Msec(10);
//Turn(10);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 0;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//while(true){}
float Angle = 0;
Angle = getUltraAngle();
/*if (Angle == -1)
Move(-5, 0.3);
else if (Angle >= 86 && Angle <= 90)
break;*/
writeDebugStreamLine("%f", Angle);
Turn(Angle - 90);
float dist = -24;
Move(dist, 0.25);
//grab goal
wait10Msec(30);
servo[hook1] = 235;
servo[hook2] = 30;
wait10Msec(100);
Turn(90 - Angle + 20);
wait10Msec(10);
Move(110, 1);
wait10Msec(10);
Turn(150);
wait10Msec(10);
//score
motor[lift] = 75;
raising = true;
timeValue = 3250;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
Move(3, 0.2);
wait10Msec(10);
servo[output] = 250;
wait10Msec(500);
//release goal
servo[hook1] = 0;
servo[hook2] = 255;
wait10Msec(300);
Move(5, 1);
wait10Msec(30);
Turn(180);
}
int calc(){
int length = SensorValue[USBack];
int width = SensorValue[USRight];
bool haveBall = TSreadState(HaveBaller);
int frontIRValue = HTIRS2readACDir(IRFront);
int backIRValue = HTIRS2readACDir(IRBack);
int currentRelCompass = HTMCreadRelativeHeading(Compass);
HTCS2readRGB(Colour, currentRed, currentGreen, currentBlue);
nxtDisplayTextLine(3, "Abs: %4d", currentRelCompass);
nxtDisplayTextLine(4, "Dir: %4d", frontIRValue);
if(currentRelCompass < -nbound){
return CC;
}
else if(currentRelCompass > nbound){
return CW;
}
else{
if(!haveBall){
if(IRFront != 0){
switch(frontIRValue){
case 1:
return B;
break;
case 2:
return BL;
break;
case 3:
return L;
break;
case 4:
return FL;
break;
case 5:
return F;
break;
case 6:
return FR;
break;
case 7:
return R;
break;
case 8:
return BR;
break;
case 9:
return B;
break;
}
}
else{
switch(backIRValue){
case 0:
return ST;
break;
case 1:
return BR;
break;
case 2:
return BR;
break;
case 3:
return B;
break;
case 4:
return BL;
break;
case 5:
return BL;
break;
case 6:
return BR;
break;
case 7:
return B;
break;
case 8:
return BL;
break;
case 9:
return BL;
break;
}
}
}
else{
hashtagyoloswagdiem();
return F;
}
}
}
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);
}
task main()
{
InitializeTeleop();
//waitForStart();
StartTask(DriveTank);
while(true)
{
nxtDisplayTextLine(2, "%d", TSreadState(touchSensor));
nxtDisplayTextLine(1, "%d", HTIRS2readDCDir(irSensor));
/*
getJoystickSettin
//Goal grabber cogs(joystick);
mmands
if(joy1Btn(8) == 1)
{
GrabGoal();
}
if(joy1Btn(7) == 1)
{
ReleaseGoal();
}
//Collector commands
if(joy2Btn(8) == 1)
{
CollectBalls();
}
else if(joy2Btn(7) == 1)
{
ReleaseBalls();
}
else
{
StopCollector();
}
//Container commands
if(joystick.joy2_TopHat == 0)
{
HoldBalls();
}
else if(joystick.joy2_TopHat == 4)
{
DumpBalls();
}
//Lift Commands
//This is for using the lift without encoders
if(joystick.joy2_y2 > ANALOG_DEAD_ZONE)
{
RaiseLift();
}
else if(joystick.joy2_y2 < -1*ANALOG_DEAD_ZONE)
{
LowerLift();
}
else
{
StopLift();
}
//This is for using the lift with the encoder
if(joy2Btn(1) == 1)
{
MoveLifter(DownPos);
}
else if(joy2Btn(2) == 1)
{
MoveLifter(LowGoalPos);
}
else if(joy2Btn(3) == 1)
{
MoveLifter(MediumGoalPos);
}
else if(joy2Btn(4) == 1)
{
MoveLifter(HighGoalPos);
}
else if(joy2Btn(10) == 1)
{
MoveLifter(CenterGoalPos);
}
*/
}
}
task main()
{
disableDiagnosticsDisplay();
while(true) {
getJoystickSettings(joystick); // Update Buttons and Joysticks
wait1Msec(10);
/*--------------------------
controller one
-------------------------*/
/*-------------------------
maping
---------------------------
up =
down =
left = strafe left
right = strafe right
joystick left = left motors
joystick right = right motors
A = catcher down
B = catcher up
X =
Y =
L1 =
L2 =
R1 =
R2 =
---------------------------*/
// If lift is too high slower the driver motors
if(nMotorEncoder[liftRight] < GOVLIMIT) {
GOVERNOR = 1;
} else {
GOVERNOR = 2;
}
// Drive the robot from joystick 1
if((abs(joystick.joy1_y1) >= deadZone) || (abs(joystick.joy1_y2) >= deadZone)) {
setMotion(joystick.joy1_y1 / GOVERNOR, joystick.joy1_y2 / GOVERNOR);
}
/*else if(joystick.joy1_TopHat == 6) {
strafe(50);
}
else if(joystick.joy1_TopHat == 2){
strafe(-50);
}*/
else if(joy1Btn(JOY_BUTTON_LT)) {
strafe(-50);
}
else if(joy1Btn(JOY_BUTTON_RT)) {
strafe(50);
}
else {
stopMotors();
}
if(joy1Btn(JOY_BUTTON_RB))
{
int iCRate = servoChangeRate[goalCapture]; // Save change rate
servoChangeRate[goalCapture] = 0; // Max Speed
servo[goalCapture] = CATCHDOWN; // Set servo position
wait1Msec(20);
servoChangeRate[goalCapture] = iCRate; // Reset the servo
}
else if(joy1Btn(JOY_BUTTON_LB))
{
int iCRate = servoChangeRate[goalCapture]; // Save change rate
servoChangeRate[goalCapture] = 0; // Max Speed
servo[goalCapture] = CATCHUP; // Set servo position
wait1Msec(20);
servoChangeRate[goalCapture] = iCRate; // Reset the servo
}
/*--------------------------
controller two
-------------------------*/
/*-------------------------
maping
---------------------------
up =
down =
left =
right =
joystick left =
joystick right =
A =
B =
X =
Y =
LB = big backward
LT = big forward
RB = small backward
RT = small forward
---------------------------*/
// Raise/lower the lift
if(abs(joystick.joy2_y2) > deadZone) {
stopLiftTask(); //first, ensure that robot is not already moving the lift
if((joystick.joy2_y2 <= 0) && TSreadState(LTOUCH)) {
lift(0);// if touch sensor is active and driver says go down, stop the lift (don't burn out motor)
nMotorEncoder[liftMotor] = 0; //The lift is down, so set lift encoder to 0
} else {//If touch is NOT active or driver says go up
lift(rescale(joystick.joy2_y2)); //Raise lift at a rescaled value of the joystick
}
} else { //No controls?
lift(0); //Stop lift motors.
}
// Set the lift to preset heights
/*if(joy2Btn(JOY_BUTTON_A)) {
liftHeight(35);
} else if(joy2Btn(JOY_BUTTON_B)) {
liftHeight(65);
} else if(joy2Btn(JOY_BUTTON_Y)) {
liftHeight(95);
} else if(joy2Btn(JOY_BUTTON_X)) {
liftHeight(115);
} else if(joy2Btn(JOY_BUTTON_RT)) {
liftHeight(0);
nMotorEncoder[liftRight] = 0;
}*/
// Run the spinner to pick up balls
if((abs(joystick.joy2_y1)) >= deadZone) {
spin(rescale(joystick.joy2_y1));
} else {
spin(0);
}
}
}
void raiseElevator ( )
{
while (!TSreadState (liftUpSensor))
motor [linearSlides]=100;
motor [linearSlides]=0;
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
//leave zone
Move(-9.66, 1);
wait10Msec(10);
Turn(28);
wait10Msec(10);
Move(-65, 1);
wait10Msec(10);
Turn(-23);
wait10Msec(10);
Move(-37, 0.9);
wait10Msec(10);
Move(-10, 0.2);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
nMotorEncoder[lift] = 0;
while (abs(nMotorEncoder[lift]) < ULTRASONIC_RAISE)
{
if (TSreadState(touch) != 1 && !raising)
{
nMotorEncoder[lift] = 0;
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//while(true){}
float Angle = 0;
Angle = getUltraAngle();
/*if (Angle == -1)
Move(-5, 0.3);
else if (Angle >= 86 && Angle <= 90)
break;*/
writeDebugStreamLine("%f", Angle);
Turn(Angle - 90);
float dist = -16;
Move(dist, 0.25);
//grab goal
wait10Msec(30);
servo[hook1] = 235;
servo[hook2] = 30;
wait10Msec(150);
Turn(90 - Angle);
wait10Msec(10);
Move(117, 1);
wait10Msec(10);
Turn(-90);
wait10Msec(10);
Move(-25, 0.5);
wait10Msec(10);
//score
motor[lift] = 75;
nMotorEncoder[lift] = 0;
while (abs(nMotorEncoder[lift]) < (LOW_GOAL - ULTRASONIC_RAISE))
{
wait10Msec(10);
}
motor[lift] = 0;
Move(3, 0.2);
wait10Msec(10);
servo[output] = 250;
wait10Msec(500);
//release goal
servo[hook1] = 0;
servo[hook2] = 255;
wait10Msec(300);
Move(5, 1);
wait10Msec(30);
Turn(90);
}