// =======================================================================
// Function to move the robot by the gyro by time sideways V2
// =======================================================================
void GyroTimeS_moveV2(int time, int power, bool light,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
relHeading =0;
Current_Angle=0; // reset current angle
long adj_power;
long adj_deg;
int i = 0;
wait1Msec(200);
motor[LF_motor] = -power;
motor[RF_motor] = -power;
motor[LB_motor] = power;
motor[RB_motor] = power;
current_power = power;
ClearTimer(T1);
bool Done = false;
int addPower = 0;
while(!Done)
{
nxtDisplayBigTextLine(4, "L %3d", nrm);
if(light == true)
{
if(nrm > light_threshold) i++;
if(i > 10) Done = true;
}
if(time1[T1] > time)
{
Done = true;
}
if(adjust == true)
{
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
addPower = (current_power*5)/100;
if(power > 0)
{
motor[LF_motor] = -(current_power-addPower); //more
motor[RF_motor] = -((current_power+addPower) - adj_power);
motor[LB_motor] = (current_power-addPower); //more
motor[RB_motor] = ((current_power+addPower) + adj_power);
}
else
{
motor[LF_motor] = -(current_power+addPower);
motor[RF_motor] = -((current_power-addPower) - adj_power);
motor[LB_motor] = (current_power+addPower);
motor[RB_motor] = ((current_power-addPower) + adj_power);
}
}
}
if(StopWhenDone==true)
{
motor[LF_motor] = 0;
motor[RF_motor] = 0;
motor[LB_motor] = 0;
motor[RB_motor] = 0;
}
}
//=========================================
// Main Program
//=========================================
task main()
{
disableDiagnosticsDisplay();
alive();
initializeRobot();
StartTask(sensors);
StartTask(sonarSensors);
StartTask(selector);
waitForStart();
StartTask(display);
constHeading = 0;
relHeading = 0;
if(calibrate != 2)
{
gyroCalTime = 3;
calibrate = 1;
while(calibrate != 2)
{
EndTimeSlice();
}
}
constHeading = 0;
relHeading = 0;
wait1Msec(time_selector*1000);
PlaySound(soundBeepBeep);
switch(MissionNumber)
{
case 1:
GyroSonar_moveV2(0, SIDE_BACK, sonarLive, 110, -60,true, true, CONSTANT);
Gyro_TurnV2(42,-15,CONSTANT);
wait1Msec(1000);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,-15,true,true,false,false);
if(currDir >= 4 && currDir <= 6) column = 2;
if(currDir < 4) column = 1;
if(currDir > 6) column = 3;
if(column == 1)
{
GyroTimeS_moveV2(300,15,true,false,true,false);
GyroTimeS_moveV2(8000,15,true,true,true,false);
}
if(column == 3)
{
GyroTimeS_moveV2(300,-15,true,false,true,false);
GyroTimeS_moveV2(8000,-15,true,true,true,false);
}
break;
case 2:
while(true)
{
nxtDisplayBigTextLine(1, "%3d", sonarLive);
nxtDisplayBigTextLine(3, "%3d", sonarLive2);
}
break;
case 8:
while(true)
{
//nxtDisplayBigTextLine(1, "S: %3d", sonarLive);
//nxtDisplayBigTextLine(3, "S2: %3d", sonarLive2);
//nxtDisplayBigTextLine(5, "G: %3f", relHeading);
}
break;
}
}
task main()
{
initializeRobot();
waitForStart();
int curGyro;
int oldGyro;
oldGyro = getGyroData(S2);
movData data;
//int offset = getOffset();
bool fieldoriented = true;
//bNxtLCDStatusDisplay = false;
while( true )
{
getJoystickSettings(joystick);
if (joystick. joy2_TopHat == 0 || joystick. joy2_TopHat == 1 || joystick. joy2_TopHat == 7) { //lift up
Motors_SetSpeed(S1, 1, 1, -100);
Motors_SetSpeed(S1, 1, 2, 100);
} else if (joystick. joy2_TopHat == 3 || joystick. joy2_TopHat == 4 || joystick. joy2_TopHat == 5){ //lift down
Motors_SetSpeed(S1, 1, 1, 60);
Motors_SetSpeed(S1, 1, 2, -60);
}else {
Motors_SetSpeed(S1, 1, 1, 0);
Motors_SetSpeed(S1, 1, 2, 0);
}
if (joy2Btn(4) == 1) //ball intake
Motors_SetSpeed(S1, 4, 1, 60);
else if (joy2Btn(2) == 1)
Motors_SetSpeed(S1, 4, 1, -60);
else
Motors_SetSpeed(S1, 4, 1, 0);
if (joy2Btn(3) == 1) //kickstand knocker-downer
servo[stand] = 240;
if (joy2Btn(1) == 1)
servo[stand] = 150;
if (joy2Btn(5) == 1) //dropper
servo[ball] = 18;
if (joy2Btn(6) == 1)
servo[ball] = 160;
//if (joy2Btn(8) == 1)
// fieldoriented = false;
// Drive Base
data.xComp = joystick.joy1_x1;
data.yComp = joystick.joy1_y1-1;
data.rot = joystick.joy1_x2;
//data.xComp = 127;
//data.yComp = 0;
//data.rot = 38;
data.xComp = (data.xComp != -128 ? data.xComp : data.xComp+1);
data.yComp = (data.yComp != -128 ? data.yComp : data.yComp+1);
data.rot = (data.rot != -128 ? data.rot : data.rot+1);
//curGyro = getGyroData(S2);
//this block of if statements is the controller dead-zone
if (data.rot < 10 && data.rot > -10)
data.rot = 0;
if (data.xComp < 10 && data.xComp > -10)
data.xComp = 0;
if (data.yComp < 10 && data.yComp > -10)
data.yComp = 0;
nxtDisplayClearTextLine(2);
nxtDisplayClearTextLine(4);
nxtDisplayBigTextLine(2, "%d", curGyro);
wait1Msec(1);
nxtDisplayBigTextLine(4, "%d", oldGyro);
//if (fieldoriented)
// oldGyro = useGyro(data, oldGyro, curGyro, offset);
if (data.rot < 2 && data.rot > -2)
data.rot = 0;
if (data.xComp < 2 && data.xComp > -2)
data.xComp = 0;
if (data.yComp < 2 && data.yComp > -2)
data.yComp = 0;
//byte speed = getSqrt(data.xComp, data.yComp) + abs(data.rot);
int speed = (int)(sqrt(data.xComp*data.xComp + data.yComp*data.yComp) + abs(data.rot)); //finds speed (dist formula)
if (speed > 127) speed = 127; //Regulates speed
drive(data, (byte)speed);
}
drive(data, 0);
}
//====================================================
// Selector
//====================================================
task selector()
{
wait1Msec(1000);
while((nNxtButtonPressed != kEnterButton))
{
if (nNxtButtonPressed == kLeftButton)
{
while(nNxtButtonPressed ==kLeftButton){};
MissionNumber = MissionNumber-1;
PlaySoundFile("! Click.rso");
}
if (MissionNumber < 1)
{
MissionNumber = 1;
}
if (MissionNumber > 20)
{
MissionNumber = 20;
}
if (nNxtButtonPressed == kRightButton)
{
while(nNxtButtonPressed ==kRightButton){};
MissionNumber = MissionNumber+1;
PlaySoundFile("! Click.rso");
}
nxtDisplayBigTextLine(2, "Mission");
nxtDisplayBigTextLine(5, "%2d", MissionNumber);
}
eraseDisplay();
string tmp = "";
StringFormat(tmp, "%3d", MissionNumber);
nxtDisplayBigStringAt(20,18,tmp);
while(nNxtButtonPressed == kEnterButton)
{}
eraseDisplay();
while(nNxtButtonPressed != kEnterButton)
{
nxtDisplayBigTextLine(6, "SecDelay");
nxtDisplayBigTextLine(4, "%3d", time_selector);
if (nNxtButtonPressed == kLeftButton)
{
while(nNxtButtonPressed ==kLeftButton){};
time_selector = time_selector-1;
PlaySoundFile("! Click.rso");
}
if (nNxtButtonPressed == kRightButton)
{
while(nNxtButtonPressed ==kRightButton){}
time_selector = time_selector+1;
PlaySoundFile("! Click.rso");
}
if(time_selector > 30) time_selector = 30;
if(time_selector < 0) time_selector = 0;
}
PlaySound(soundException);
wait1Msec(200);
eraseDisplay();
while(nNxtButtonPressed == kEnterButton){}
while(nNxtButtonPressed != kEnterButton)
{
nxtDisplayBigTextLine(6, "gyro_cal");
nxtDisplayBigTextLine(4, "%2d", gyroCalTime);
if(nNxtButtonPressed == kLeftButton)
{
while(nNxtButtonPressed == kLeftButton){}
gyroCalTime = gyroCalTime-1;
PlaySoundFile("! Click.rso");
}
if(nNxtButtonPressed == kRightButton)
{
while(nNxtButtonPressed == kRightButton){}
gyroCalTime = gyroCalTime+1;
PlaySoundFile("! Click.rso");
}
if(gyroCalTime > 15) gyroCalTime = 15;
if(gyroCalTime < 0) gyroCalTime = 0;
}
if(gyroCalTime > 0) calibrate = 1;
PlaySound(soundException);
eraseDisplay();
}
task main () {
int X = 0;
memset(data, 0, sizeof(data));
TIRresetSensor(TIR);
nxtDisplayCenteredTextLine(0, "Dexter Industries");
nxtDisplayCenteredTextLine(1, "Thermal Infrared");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect sensor");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
eraseDisplay();
// set emissivity for light skin
TIRsetEmissivity(TIR, TIR_EM_SKIN_LIGHT);
nMotorEncoderTarget[VERTICAL] = 200;
motor[VERTICAL] = -20;
while((nMotorRunState[VERTICAL] != runStateIdle) && (nMotorRunState[VERTICAL] != runStateHoldPosition)) EndTimeSlice();
nMotorEncoderTarget[HORIZONTAL] = 360;
motor[HORIZONTAL] = 20;
while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition)) EndTimeSlice();
wait1Msec(500);
nMotorEncoder[HORIZONTAL] = 0;
nMotorEncoder[VERTICAL] = 0;
PlaySound(soundBeepBeep);
while(bSoundActive) EndTimeSlice();
for (int i = 0; i < 80; i++) {
wait1Msec(500);
X = 0;
memset(data, 0, sizeof(data));
nMotorEncoderTarget[HORIZONTAL] = 720;
motor[HORIZONTAL] = -40;
time1[T1] = 0;
while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition)) {
data[X] = TIRreadObjectTemp(TIR);
X++;
wait1Msec(20);
}
nxtDisplayBigTextLine(1, "X: %d", X);
nxtDisplayBigTextLine(3, "T: %d", time1[T1]);
nMotorEncoderTarget[VERTICAL] = 5;
motor[VERTICAL] = 20;
nMotorEncoderTarget[HORIZONTAL] = 720;
motor[HORIZONTAL] = 60;
for (int j = 0; j < 200; j++) {
if (data[j] != 0) {
writeDebugStream("%3.2f,", data[j]);
wait1Msec(5);
}
}
writeDebugStreamLine("");
while((nMotorRunState[HORIZONTAL] != runStateIdle) && (nMotorRunState[HORIZONTAL] != runStateHoldPosition))
EndTimeSlice();
}
bFloatDuringInactiveMotorPWM = true;
wait1Msec(10);
}
task main()
{
int elevatorHeightTicks;
// Set the following variables to false to hide the standard NXT LCD information
bDisplayDiagnostics = false;
bNxtLCDStatusDisplay = false;
////////// INITIALIZATIONS //////////
initializeRobot(); // Execute robot initialization routine
/////////////// Variables to be used later /////////////////
//int maxArmHeightTicks = inchesToTicks(maxArmHeight);
//int minRingPickupHeightTicks = inchesToTicks(minRingPickupHeight);
//int WAMservoStep = 3; //Amount to inc1rement the WAM servo position
//int WAMservoStep12 = 17; // Move a servo 15 degrees
//float maxArmHeight = 45.25; // Maximum Safe Arm Height used during manual control of the arm
//float minRingPickupHeight = 8.0;
// User selected Autonomous information
{
selectStartLocation(); // Get the starting location of the robot
selectAutoActions(); // Get Autononmous actions
switch(AutoActions)
{
case 0: // No Autonomous
break;
case 1: // IR Beacon
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 2: // Left Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 3: // Center Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 4: // Right Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
case 5: // Play Defense
selectAutoStartDelay(); // select the autonomous start delay
break;
case 6: // Left Colmnn
selectRow(); // runs the select row function
selectAutoStartDelay(); // select the autonomous start delay
break;
default: // Not a valid selection
eraseDisplay();
nxtDisplayCenteredTextLine(1,"DITU SAYS");
nxtDisplayCenteredTextLine(2,"INPUT ERROR");
nxtDisplayCenteredTextLine(4,"TRY AGAIN");
wait1Msec(6000);
break;
}
}
eraseDisplay();
// Set the following variables to false to hide the standard NXT LCD information
// Show the default FTC Display Information
bDisplayDiagnostics = true;
bNxtLCDStatusDisplay = true;
//bDisplayDiagnostics = false;
//bNxtLCDStatusDisplay = false;
// Determine the autonomous start delay based on delayAutoStart
switch(delayAutoStart)
{
case 0: // delay start = 0 seconds
delayAutoStartValue = 0;
break;
case 1: // delay start = 1 second
delayAutoStartValue = 1000;
break;
case 2: // delay start = 5 seconds
delayAutoStartValue = 5000;
break;
case 3: // delay start = 10 seconds
delayAutoStartValue = 10000;
break;
case 5: // delay start = 15 seconds
delayAutoStartValue = 15000;
break;
default: // delay start = 0 seconds
delayAutoStartValue = 0;
break;
}
// Process robot starting location selection
switch(robotStartLocation)
{
case 0: // Left Wall
leftWall = true;
break;
case 1: // Corner
corner = true;
break;
case 2: // Right Wall
rightWall = true;
break;
default: // Not a valid starting location
break;
}
// Process robot starting location selection
switch(Row)
{
case 0: // Bottom Row
pegHeightCmd = 1;
break;
case 1: // Middle Row
pegHeightCmd = 2;
break;
case 2: // Top Row
pegHeightCmd = 3;
break;
default: // Not a valid starting location
break;
}
waitForStart(); // Wait for the signal to start from the Field Control System
//reads the protoboard to see which switches are pressed. Since they are globals, nothing is returned. We may want to make this it's own task eventually
ProcessProto();
switch(AutoActions)
{
case 0: // No Autonomous
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 1 || Score Ring on the column designated by the IR Beacon===============================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 1: // Score Ring on the column designated by the IR Beacon
wait1Msec(delayAutoStartValue);
// INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE
// move forward, turn 45 degrees, move forward
move_forward(24, 2000, 90, 90);
wait10Msec(50);
//move(1, 0, 1000);
clearTimer(T1);
//HTIRS2setDSPMode(InfraredSensor, DSP_1200);
while (time1[T1] < 500)
{
irSensorVal = SensorValue[IR];
nxtDisplayBigTextLine(2, "IR: %d", irSensorVal);
}
//////////////////////////////////////////////////////////////////
switch(irSensorVal)
{
case 1:
turngyro_left(-90.0, 35);
wait10Msec(100);
move_forward(24, 4000, 90, 90);
wait10Msec(100);
turngyro_left(83.0, 30);
wait10Msec(100);
move_forward(.5, 2000, 90, 90);
wait1Msec(50);
break;
//------------------------------------------------------------------
case 2:
wait10Msec(100);
move_backwards(2, 1000, 90, 90);
turngyro_left(-45.0, 50);
wait10Msec(50);
ClearTimer(T1);
nMotorEncoder[Right2] = 0;
nMotorEncoder[Left2] = 0;
move_forward(56, 4000, 90, 90);
ClearTimer(T1);
move_backwards(12.5, 2000, 90, 90);
wait1Msec(50);
break;
//-----------------------------------------------------
case 3:
move_forward(43, 2000, 90, 90);
wait10Msec(100);
turngyro_left(-90.0, 50);
wait10Msec(100);
move_forward(2, 1000, 90, 90);
wait1Msec(50);
break;
}
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
wait1Msec(500);
// Insert robot move commands here
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeight(elevatorHeightTicks); // Score Ring
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 2 || Score Ring on Left Column==========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 2: // Score Ring on Left Column
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
wait1Msec(500);
// Insert robot move commands here
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeight(elevatorHeightTicks); // Score Ring
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 3 || Score Ring on Center Column=========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 3: // Score Ring on Center Column
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 10000); // Raise elevator to the commanded height
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
move_forward(60-5, 5000, 90, 90);
/////The alignment from the wall that could work.../////
//move_forward(24, 4000, 80, 80);
//turngyro_left(45, 60, 60);
//move_forward(4, 2000, 80, 80);
RAM_down();
// Drive forward until the RAM limit switch is activated or time expires
ClearTimer(T1);
while(ramLimit==0 && time1[T1] < 2500)
{
// Try to follow the white navigation tape
if(SensorValue(colorSensorVal) >= 15)
{
// The color sensor sees the black platform, command the robot to drift to the right
motor[Left1] = 35;
motor[Left2] = 35;
motor[Right1] = 15;
motor[Right2] = 15;
}
else
{ // The color sensor sees the white navigation tape, command the robot to drift to the left
motor[Left1] = 15;
motor[Left2] = 15;
motor[Right1] = 35;
motor[Right2] = 35;
}
ProcessProto();
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -25;
motor[Left2] = -25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 20;
motor[Left2] = 20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 25;
motor[Left2] = 25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
/*ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -20;
motor[Left2] = -20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}*/
motor[RAMright] = 0;
motor[RAMleft] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
motor[Right1] = 0;
motor[Right2] = 0;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring
if (tripped) //If the elevator trips the fail safe, break
break;
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 4 || Score Ring on Right Column=========================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 4: // Score Ring on Right Column
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
wait1Msec(500);
// Insert robot move commands here
//asdfjkl;
wait1Msec(500);
move_forward(12, 5000, 90, 90);
turngyro_right(45, 100);
move_forward(20, 7000, 80, 80);
//RAM_down();
motor[Right1] = 0;
motor[Right2] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
// Drive forward until the RAM limit switch is activated or time expires
ClearTimer(T1);
while(ramLimit==0 && time1[T1] < 2500)
{
// Try to follow the white navigation tape
if(SensorValue(colorSensorVal) >= 15)
{
// The color sensor sees the black platform, command the robot to drift to the right
motor[Left1] = 35;
motor[Left2] = 35;
motor[Right1] = 15;
motor[Right2] = 15;
}
else
{ // The color sensor sees the white navigation tape, command the robot to drift to the left
motor[Left1] = 15;
motor[Left2] = 15;
motor[Right1] = 35;
motor[Right2] = 35;
}
ProcessProto();
}
ClearTimer(T1);
while(time1[T1] < 1500)
{
motor[Left1] = -25;
motor[Left2] = -25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 1500)
{
motor[Left1] = 20;
motor[Left2] = 20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 1500)
{
motor[Left1] = 25;
motor[Left2] = 25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -20;
motor[Left2] = -20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
motor[RAMright] = 0;
motor[RAMleft] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
motor[Right1] = 0;
motor[Right2] = 0;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring
if (tripped) //If the elevator trips the fail safe, break
break;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height to score a ring
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeight(elevatorHeightTicks); // Score Ring
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 5 || Play Defense=======================================================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 5: // Play Defense
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
elevatorGoToHeight(elevatorHeightTicks); // Raise elevator to the commanded height
//wait1Msec(500);
move_backwards(84.0, 10000, 100, 100);
break;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Case: 6 || Score Center & Line Up with the Dispsenser=========================================================//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
case 6: // Score on the center column & line up with the dispenser
wait1Msec(delayAutoStartValue);
elevatorHeightTicks = selectLocation(5); // Determine the number of encoder ticks to raise the elevator to the drive height
tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 10000); // Raise elevator to the commanded height
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
move_forward(60-5, 5000, 90, 90);
RAM_down();
// Drive forward until the RAM limit switch is activated or time expires
ClearTimer(T1);
while(ramLimit==0 && time1[T1] < 2500)
{
// Try to follow the white navigation tape
if(SensorValue(colorSensorVal) >= 15)
{
// The color sensor sees the black platform, command the robot to drift to the right
motor[Left1] = 35;
motor[Left2] = 35;
motor[Right1] = 15;
motor[Right2] = 15;
}
else
{ // The color sensor sees the white navigation tape, command the robot to drift to the left
motor[Left1] = 15;
motor[Left2] = 15;
motor[Right1] = 35;
motor[Right2] = 35;
}
ProcessProto();
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = -25;
motor[Left2] = -25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 20;
motor[Left2] = 20;
motor[Right1] = -20;
motor[Right2] = -20;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
ClearTimer(T1);
while(time1[T1] < 200)
{
motor[Left1] = 25;
motor[Left2] = 25;
motor[Right1] = 25;
motor[Right2] = 25;
motor[RAMright] = 95;
motor[RAMleft] = 95;
}
motor[RAMright] = 0;
motor[RAMleft] = 0;
motor[Left1] = 0;
motor[Left2] = 0;
motor[Right1] = 0;
motor[Right2] = 0;
// Now that the robot is in the scoring location, raise the elevator to the commanded row
elevatorHeightTicks = selectLocation(pegHeightCmd); // Determine the number of encoder ticks based on the commanded elevator height
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Raise elevator to the commanded height to score a ring
if (tripped) //If the elevator trips the fail safe, break
break;
wait1Msec(500);
elevatorHeightTicks = selectLocation(4); // Determine the number of encoder ticks to lower the elevator and score the ring
elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000); // Score Ring
if (tripped) //If the elevator trips the fail safe, break
break;
move_backwards(40-5, 5000, 90, 90);
turngyro_right(135, 100);
move_forward(20-2, 5000, 90, 90);
turngyro_left(90, 100);
move_forward(10-2, 5000, 90, 90);
turngyro_left(90, 100);
move_backwards(10-2, 5000, 90, 90);
break;
default: // Not a valid autonomous action
break;
}
servo[colorSensorServo] = colorServoDefault;
wait10Msec(200);
// All Done, time to stop all tasks
StopAllTasks();
} // End Main Bracket
task main() {
int _chVal = 0; // analog input
byte inputs = 0; // all digital inputs
int value = 0;
int touchValue = 0;
int touchValue2 = 0;
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Proto");
nxtDisplayCenteredTextLine(3, "Test 1");
nxtDisplayCenteredTextLine(5, "Connect HTPB");
nxtDisplayCenteredTextLine(6, "to S1");
wait1Msec(2000);
// Setup all the digital IO ports as outputs (0x30) 110000 pins B4/B5 = outputs, others are inputs.
if (!HTPBsetupIO(HTPB, 0x30)) {
nxtDisplayTextLine(4, "ERROR!!");
wait1Msec(2000);
StopAllTasks();
}
while(true) {
value = 0;
eraseDisplay();
// get the value for ADC channel 0, we want a 10 bit answer
int j = 0;
for(int i=0; i<5; i++)
{
_chVal = HTPBreadADC(HTPB, j, 10);
switch(j) {
case 0: nxtDisplayTextLine(0, "A0: %d", _chVal);
if(_chVal>512) value+=(1<<8);
break;
case 1: nxtDisplayTextLine(1, "A1: %d", _chVal);
if(_chVal>512) value+=(1<<7);
break;
case 2: nxtDisplayTextLine(2, "A2: %d", _chVal);
if(_chVal>512) value+=(1<<6);
break;
case 3: nxtDisplayTextLine(3, "A3: %d", _chVal);
if(_chVal>512) value+=(1<<5);
break;
case 4: nxtDisplayTextLine(4, "A4: %d", _chVal);
if(_chVal>512) value+=(1<<4);
break; }
j++;
}
inputs = HTPBreadIO(HTPB, 0x3F);
nxtDisplayTextLine(5, "D: 0x%x", inputs);
value+=(inputs&0x01)<<3;
value+=(inputs&0x02)<<1;
value+=(inputs&0x04)>>1;
value+=(inputs&0x08)>>3;
nxtDisplayBigTextLine(6, "%d", mapNineBitToDegrees(value));
touchValue = SensorValue[S2];
touchValue2 = SensorValue[S3];
if(touchValue == 1 && touchValue2 == 1) { HTPBwriteIO(HTPB, 0x30); } // turn on B4 and B5
else if(touchValue == 1) { HTPBwriteIO(HTPB, 0x10); } // turn on B4
else if(touchValue2 == 1) { HTPBwriteIO(HTPB, 0x20); } // turn on B5
else { HTPBwriteIO(HTPB, 0x00); } // turn both B4 and B5 off
wait1Msec(50);
}
}
task main()
{
while(true){
nxtDisplayBigTextLine(1, "%d", SensorValue[irseek]);
}
}
// Realiza el seguimiento de la pared lateral situada a la derecha
// del robot. El robot mantiene el seguimiento a una distancia de seguridad
// hasta que se encuentre con una pared frontal. En este caso se detiene y
// realiza un giro de 90 grados hacia la izquierda
void track_wall()
{
float distanciaParaParar = 40;
float distanciaAlMuro = 40;
float umbral = 0;
float v = 0.2;
float w = 0;
// Para evitar chocar contra un muro frontal
while(SensorValue[sonarSensorFrontal] > distanciaParaParar)
{
AcquireMutex(semaphore_odometry);
float x = robot_odometry.x;
float y = robot_odometry.y;
float th = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
float sval = SensorValue[sonarSensorLateral];
string temp;
StringFormat(temp, "%.2f", sval);
nxtDisplayBigTextLine(2, temp);
string temp2;
StringFormat(temp2, "%.2f", th);
nxtDisplayBigTextLine(5, temp2);
// Oscila a una distancia segura de la pared lateral
// Debe girar a la derecha porque se esta alejando
if (SensorValue[sonarSensorLateral] > distanciaAlMuro + umbral) {
w = -v;
if(th < -0.2){
w = 0;
}
}
// Debe girar a la izquierda porque se esta acercando
else if(SensorValue[sonarSensorLateral] < distanciaAlMuro - umbral) {
w = v;
if(th > 0.2){
w = 0;
}
} else {
if(th > 0.2){
w = -(v*0.9);
} else if(th < -0.2){
w = v*0.9;
} else{
w = 0;
}
}
setSpeed(v,w);
}
// Gira al encontrarse con un muro de frente
float theta, thetaFinal;
float errorTheta = 0.005;
setSpeed(0,0);
// condicion de parada
AcquireMutex(semaphore_odometry);
float x = robot_odometry.x;
float y = robot_odometry.y;
float th = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
theta = th;
thetaFinal = (PI)/2;
PlaySoundFile("wilhelmA.rso");
Sleep(1000);
v = 0;
w = 1.5;
setSpeed(v,w);
while(abs(theta - thetaFinal) > errorTheta) {
AcquireMutex(semaphore_odometry);
theta = robot_odometry.th;
ReleaseMutex(semaphore_odometry);
}
}
