void init()
{
bSmartDiagnostics = true; //true to enable smart diagnostic screen
bCompetitionMode = true; //true to enable competition mode
displaySplash("Mecanum Bot", "", true);
bool ok = false;
while(!ok)
{
const int testcount = 2;
bool test[testcount] = {
errorcheck(1,0,1,MOTORCON),
errorcheck(1,0,2,MOTORCON)};
string desc[testcount] = {"MC1-1","MC1-2"};
ok = error_display(test,desc,testcount);
if (!ok) {
PlayTone(440, 50);
if (test[0] == false && test[1] == false){
nxtDisplayCenteredTextLine(7, "Reboot MC!");
}
}
else { ClearSounds(); }
}
eraseDisplay();
gyro_init(HTGYRO);
wait1Msec(50);
nxtbarOn();
return;
}
void display_menu(){
//bool we use to keep from redrawing everything
run_once=false;
drawMenu();
wait1Msec(250);
//while we are not pressing enter
while(nNxtButtonPressed!=3){
//if we press the left button
//and it has been .5 seconds since the last button press
//and we have an upper menu to go to
if(nNxtButtonPressed==2 && time1[T1] > 500 && selection > 0){
time1[T1] = 0; //reset time 1 so that we don't repeat
change_menu(-1); //decrease selection
}else if(nNxtButtonPressed==1 && time1[T1] > 500 && selection < 7){ //same as above but with right button and making sure we have a menu item below us
change_menu(1); //increase selection
time1[T1] = 0; //reset time
}
}
while(nNxtButtonPressed==3); //do not double read enters so hang while holding enter
eraseDisplay(); //clear display
switch(selection) //get delection and make decision
{
case 0: //go back
current_function=previous_function;//go back to prev page
ClearSounds(); //clear sound buffer. not neccesary probably
break;
case 1:
current_function=0;//log
break;
}
wait1Msec(250); // don't immediately move on wait .25 seconds
}
sub MTASK_DOTASK(int MTASK_ID){
switch (MTASK_ID)
{
//********
case MT_DEFAULT:
wait1Msec(1);
break;
//********
case MT_BEEP:
PlayTone(200, 12);
wait10Msec(120);
break;
//********
case MT_STOP_BUTTON:
if(nNxtButtonPressed==BT_ENTER)
{
int static TimeDif;
TimeDif=time10[T4];
while(nNxtButtonPressed==BT_ENTER){
if(time10[T4]-TimeDif>50)
{
MV_StopMotors();
ClearSounds();
PlaySound(soundBlip);
ESTADO_SET_TARGET(ST_WAIT);
break;
}
}
}
break;
//********
case MT_ACCEL:
static int LastTime;
if(time100[T4]-LastTime >= 3)
{
HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]);
if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){
if (ACCEL_Filter++ > 2)
ACCEL_Rampa = true;
}else{
ACCEL_Filter = 0;
ACCEL_Rampa = false;
}
LastTime=time100[T4];
}
break;
//********
case MT_TOQUE:
wait1Msec(1);
break;
}
}
void liftControl() {
if (isLiftUp())
{
liftUp();
PlaySound(soundUpwardTones);
}
else if (isLiftDown())
{
liftDown();
PlaySound(soundDownwardTones);
}
else
{
liftStop();
ClearSounds();
}
}
task main()
{
//wait
servo[topServo] = 235;
initializeRobot();
StartTask(getHeading);
wait1Msec(400);
forwardInc(48, 30);
//Calculates the positioning of the center goal after 10 position tics at 200msec intervals
float a = 0;
float b = 0;
float c = 0;
wait1Msec(400);
StartTask(infared);
wait1Msec(200);
int ir2 = acS1a+ acS2a+acS3a+acS4a+acS5a;
int ir1 = acS1b+acS2b+acS3b+acS4b+acS5b;
for(float i = 0; i <= 10; i++){
if(ir1 < ir2-10){
PlayImmediateTone(440, 10);
a += 1;
}else if(abs(ir1-ir2) <= 18){ //ir1-4 > ir2 && ir1+4 < ir2
PlayImmediateTone(1024, 10);
b += 1;
}else if(ir1 > ir2+10){
PlayImmediateTone(2024, 10);
c += 1;
}else{
PlayImmediateTone(1024, 10);
//b += 1;
}
wait1Msec(200);
}
ClearSounds();
//code for each of the three center positions a=1 b=2 c=3
StopTask(getHeading);
wait10Msec(50);
StartTask(getHeading);
wait1Msec(200);
if(a > (b+c)/2){
//PlayImmediateTone(440, 10);
currHeading = 0;
turnDeg(-20, 25);
wait10Msec(50);
forwardInc(74, 30);
wait10Msec(50);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-52, 20);//sdfhjkjhgftrhjkl;kjrtykl;kjgfdsghj.,mhfdsfghjkfdsgtgr5gtgr
wait10Msec(25);
forwardInc(-22, 25);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(500);
liftPos(0);
wait10Msec(500);
}else if(b > (a+c)/2){
PlayImmediateTone(1024, 10);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(180, 25); //do a one 80
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait10Msec(70);
forwardInc(-36, 30);//go back __ inches
//StartTask(getHeading);
//wait1Msec(100);
//turnDeg(20, 25);
wait10Msec(200);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(100);
liftPos(0);
wait10Msec(500);
}else if(c > (a+b)/2){
PlayImmediateTone(2024, 10);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-120, 25);
StopTask(getHeading);
currHeading = 0;
wait1Msec(200);
StartTask(getHeading);
wait10Msec(70);
forwardInc(-35, 30);
StartTask(getHeading);
wait1Msec(100);
turnDeg(-32, 25);
forwardInc(-4, 25);
wait10Msec(50);
liftPos(3);
servo[topServo] = 210;//original 150
wait10Msec(25);
servo[topServo] = 235;
wait10Msec(100);
liftPos(0);
wait10Msec(500);
}
}
sub MTASK_DOTASK(int MTASK_ID){
switch (MTASK_ID)
{
//********
case MT_DEFAULT:
wait1Msec(1);
break;
//********
case MT_BEEP:
PlayTone(200, 12);
wait10Msec(120);
break;
//********
case MT_STOP_BUTTON:
if(nNxtButtonPressed==BT_ENTER)
{
int static TimeDif;
TimeDif=time10[T4];
while(nNxtButtonPressed==BT_ENTER){
if(time10[T4]-TimeDif>50)
{
MV_StopMotors();
ClearSounds();
PlaySound(soundBlip);
ESTADO_SET_TARGET(ST_WAIT);
break;
}
}
}
break;
//********
case MT_ACCEL:
if(!getSafe(3))
{
hogCPU();
HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]);
releaseCPU();
if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){
if (ACCEL_Filter++ > 2){
ACCEL_Rampa = true;
ACCEL_Filter = 3;
}
}else{
if (ACCEL_Filter-- < 2){
ACCEL_Filter = 0;
ACCEL_Rampa = false;
}
}
setSafe(3,300);
}
break;
//********
case MT_TOQUE:
wait1Msec(1);
break;
//********
case MT_LL:
hogCPU();
LL_Avr = LLreadAverage(PORT_LL);
LL_IO = LLreadResult(PORT_LL);
releaseCPU();
wait1Msec(5);
break;
//********
case MT_US:
wait10Msec(3);
hogCPU();
ReadAllUS_short(PORT_ARD,USwall);
releaseCPU();
break;
}
}
task main()
{
/*
* INITIALIZATION AND SETUP
* This first section gets the robot ready to start the match. It sets all the motor and servo
* starting positions, and prints some diagnostic information to the NXT and the debug stream.
*/
// Print a copyright notice to the debug stream
string programName = "Autonomous";
printWelcomeMessage(programName, programVersion);
writeDebugStreamLine("Getting autonomous settings...");
PlaySound(soundBeepBeep);
runMenu(); // Get gameplay decisions from the operators
initializeRobot(); // Set the robot to its starting positions
// Notify the users that the program is ready and running
writeDebugStreamLine("Waiting for start of match...");
nxtDisplayTextLine(0, "2015 Powerstackers");
nxtDisplayCenteredBigTextLine(1, "AUTO");
nxtDisplayCenteredBigTextLine(3, "READY");
PlaySound(soundFastUpwardTones);
wait10Msec(200);
/*
* DEBUG MODE
* If debug mode is activated, bypass the waitForStart function.
* Display a 3 second audio and visual countdown, and then execute.
*/
if(debugMode)
{
ClearSounds();
for(int i = 3; i>0; --i)
{
nxtDisplayCenteredBigTextLine(3, "IN %d", i);
PlaySound(soundException);
wait10Msec(100);
}
}
else
{
// Wait for the "starting gun" from the field control system
waitForStart();
}
// Notify the users that the program is running
nxtDisplayCenteredBigTextLine(3, "RUNNING");
PlaySound(soundUpwardTones);
//StartTask(watchMotors);
/*
* GAMEPLAY
* From this point on, the program is split into different sections based on the options chosen in
* menu earlier. The two main options are the starting position and the game mode (offense or defense).
*/
if(startingPosition==STARTING_RAMP)
{
/*
* RAMP POSITION
* Starting from this position means that you can only access the rolling goals and the kickstand.
* In this position, offensive is mostly the only available game mode.
*/
// OFFENSIVE MODE
if(offenseOrDefense==OFFENSIVE_MODE)
{
// Go straight down the ramp
goTicks(inchesToTicks(-25), 30);
turnDegrees(5, 50);
goTicks(inchesToTicks(-40), 50);
//this will make the robot drop the balls into the rolling goal
grabTube();
//this will make the robot turn to move twards the parking zone
turnDegrees (-8,preferredTurnSpeed);
goTicks(inchesToTicks(12), 75);
turnDegrees(-20, preferredTurnSpeed);
//this will make the robot move twards the parking zone
goTicks (inchesToTicks(78), 75);
//this will make the robot turn so it can be in place so it will move in the parking zone
turnDegrees (-80,preferredTurnSpeed);
goTicks(inchesToTicks(10), 50);
dropBall(liftTargetMed);
} // END OFFENSE
// DEFENSIVE MODE
else if(offenseOrDefense==DEFENSIVE_MODE)
{
// Just go in a straight line down the ramp
goTicks(inchesToTicks(80), 50);
} // END DEFENSE
} // END RAMP START
else if(startingPosition==STARTING_FLOOR)
{
/*
* FLOOR POSITION
* In this position, you are available to block the other team, or to score in the high goal. You
* also have easy access to the kickstand. It is easiest to determine the orientation of the
* center tower from this position.
*/
// Detect and store the center goal position
goTicks(inchesToTicks(-30), 100);
char goalFacing = findGoalOrientation();
// OFFENSIVE MODE
if(offenseOrDefense==OFFENSIVE_MODE)
{
// Move slightly forwards to get into a better detecting position
//goTicks(inchesToTicks(-20), 50);
// Make a movement based on the position of the center goal
switch (goalFacing)
{
// GOOD
case CENTGOAL_POSITION_A :
{
// In this position, simply go in a straight line
goTicks(inchesToTicks(-10),50);
break;
}
// BAD
case CENTGOAL_POSITION_B :
{
// Turn 45 degrees, tangent to the center structure
turnDegrees (45,preferredTurnSpeed);
// Move into position in front of the goal
goTicks(inchesToTicks(-30),75);
// Turn to face the goal
turnDegrees (-90,preferredTurnSpeed); writeDebugStreamLine("Done");
break;
}
// GOOD
case CENTGOAL_POSITION_C :
{
// Turn 90 degrees, parallel to the center structure
turnDegrees (-90,preferredTurnSpeed);
// Move along the center structure until we're past the end
goTicks(inchesToTicks(-38),50);
// Turn 90 degrees, tangent to the center structure
turnDegrees (90,preferredTurnSpeed);
// Move into position in front of the center structure
goTicks(inchesToTicks(-62),65);
// Turn to face the goal
turnDegrees(90,preferredTurnSpeed);
break;
}
default:
{
// In an ambiguous situtation, assume the goal is in A position
goTicks(inchesToTicks(-20),75); writeDebugStreamLine("Done");
break;
}
}
// Print that the alignment is done
writeDebugStreamLine("Aligned with center structure");
// Position the robot to drop the ball in the center goal
nMotorEncoder[mLift] = 0;
moveMotorTo(mLift, liftTargetCent, 75);
// Drop the ball in the center goal
servo[rTrapDoor]=trapDoorOpenPosition;
wait10Msec (500);
servo[rTrapDoor]=trapDoorClosedPosition;
// Drop down the lift
moveMotorTo(mLift, liftTargetBase, 75);
// Position the robot correctly to kick the kickstand
turnDegrees (90, preferredTurnSpeed);
goTicks(inchesToTicks(-15), 75);
turnDegrees (90, preferredTurnSpeed);
goTicks(inchesToTicks(35), 100);
// Go to the robot's ending position
} // END OFFENSE
// DEFENSIVE MODE
else if(offenseOrDefense==DEFENSIVE_MODE)
{
/*
* Move from the starting position to block the opponent's rolling goals
*/
// Move 2 feet backwards at full power, to get out of the parking zone
//goTicks(inchesToTicks(-24), 100);
// Turn towards the opponent rolling goals
turnDegrees(90, preferredTurnSpeed);
// Drive forward and backward to disrupt the opponent 60cm goal
goTicks(inchesToTicks(60), 100);
goTicks(inchesToTicks(-12), 100);
// turn slightly to get the 90cm goals
turnDegrees(-45, preferredTurnSpeed);
// Move forward and backwards to disrupt the 90cm goal
goTicks(inchesToTicks(36), 75);
goTicks(inchesToTicks(-36), 100);
return;
// Turn slightly to face the 30cm goal
turnDegrees(-45, preferredTurnSpeed);
// Move foward and backward to disrupt the 30cm goal
goTicks(inchesToTicks(36), 75);
goTicks(inchesToTicks(-4), 100);
// Move into position to align on the wall of the ramp
turnDegrees(-90, preferredTurnSpeed);
//goTicks(inchesToTicks(12), 100);
//turnDegrees(90, preferredTurnSpeed);
return;
// Align ourselves with this wall
//wallAlign(ALIGN_FORWARD);
/*
* BLOCK CETNER GOAL
* Move the robot in front of the opponent's center goal
*/
// Move into position for use to block the center goal
goTicks(inchesToTicks(-6), 100); // Move away from the wall
turnDegrees(90, preferredTurnSpeed); // Turn to face OUR ramp
goTicks(inchesToTicks(-36), 100); // Move foward, past the center goal
//turnDegrees(90, preferredTurnSpeed); // Turn parallel to the cetner structure
// Move to block the center goal, in one of three positions
switch(goalFacing)
{
// Farthest position
case CENTGOAL_POSITION_A:
{
writeDebugStreamLine("-- BLOCKING GOAL POSITION A --");
goTicks(inchesToTicks(-80), 100); // Move alongside the goal
turnDegrees(90, preferredTurnSpeed); // Turn tangent to the center structure
goTicks(inchesToTicks(20), 100); // Move in front of the goal
break;
}
// Middle position
case CENTGOAL_POSITION_B:
{
writeDebugStreamLine("-- BLOCKING GOAL POSITION B --");
goTicks(inchesToTicks(-40), 100); // Move alongside the goal
turnDegrees(45, preferredTurnSpeed); // Turn tangent to the center structure
goTicks(inchesToTicks(20), 100); // Move in front of the goal
break;
}
// Closest position
case CENTGOAL_POSITION_C:
{
writeDebugStreamLine("-- BLOCKING GOAL POSITION C --");
goTicks(inchesToTicks(-20), 100); // Move in front of the goal
break;
}
default:
{
break;
}
}
} // END DEFENSE
} // END FLOOR START
// After the program has been carried out, play a cute "done" sound
nxtDisplayCenteredBigTextLine(3, "DONE");
PlaySound(soundBeepBeep);
wait10Msec(200);
} // END
