void pulse_backward(){ move_backwards(10); current_pulse_delay = 200; start_pulse_timer(100); pulse_fn = pulse_backward; is_pulsing = true; }
static void console_out_character(console_private_t *pcp, const unsigned char ch) { cursor(pcp, CS_SUSPEND); if (!pcp->bMarkMode) { if (pcp->bIsMarked) { unmark_window(pcp); pcp->bIsMarked = FALSE; } } if (ch >= 32) { pcp->pchWindowBuffer[CALC_POS(pcp, pcp->xPos, pcp->yPos)] = ch; draw_current_character(pcp); move_forwards(pcp); } else { /* do we have a backspace? */ if (ch == 8) { move_backwards(pcp); pcp->pchWindowBuffer[CALC_POS(pcp, pcp->xPos, pcp->yPos)] = ' '; draw_current_character(pcp); } /* do we have a return? */ if ( (ch == 13) || (ch == '\n')) { pcp->xPos = 0; move_downwards(pcp); } } cursor(pcp, CS_RESUME); }
void backing_away_from_depo_fn(){ if (just_entered_state){ debug_red->led_on(); start_timer(MAIN_STATE_TIMER, 600); move_backwards(10); } if (respond_to_timer(MAIN_STATE_TIMER, POST_ARC_SERVER_SEARCH)) return; }
void moving_backward_fn(){ if (just_entered_state){ debug_green->led_on(); Serial.println("moving_backward_fn"); start_timer(MAIN_STATE_TIMER, 2000); move_backwards(2); // move_forwards(5); } if (respond_to_key(ROTATING_LEFT)) return; if (respond_to_timer(MAIN_STATE_TIMER, NULL_STATE)); }
void straight_back_to_depo_fn(){ if(just_entered_state){ start_state_init_timer(1000); // wait for last coin } if (state_init_timer_finished()){ start_timer(MAIN_STATE_TIMER, 5000); move_backwards(10); // wheeeeee // pray for straight movement debug_blue->led_on(); } if (state_init_finished){ if (respond_to_timer(MAIN_STATE_TIMER, LIFTING_HOPPER)) return; } }
void finding_depo_to_skip_fn(){ if (just_entered_state){ debug_red->led_on(); move_backwards(9); start_state_init_timer(700); // back up a bit first } if (state_init_timer_finished()){ debug_blue->led_on(); debug_red->led_off(); if (arena_side == RIGHT_SIDE){ // rotate_right(5); pulse_rotate_right(); direction_of_interest = DIR_RIGHT; // pulse_fine_rotate_right(); } else if (arena_side == LEFT_SIDE){ // rotate_left(5); pulse_rotate_left(); direction_of_interest = DIR_LEFT; // pulse_fine_rotate_left(); } else { // rotate_right(5); pulse_rotate_right(); direction_of_interest = DIR_RIGHT; } } if (state_init_finished){ check_pulse(); // if (respond_to_tape_on(tape_f, FINAL_MOVE_TO_DEPO)) return; // if (respond_to_tape_on(tape_f, MOVING_TO_DEPO)) return; if (respond_to_depository_found(FINAL_MOVE_TO_DEPO)) { start_timer(SECONDARY_TIMER, 10000);// uh oh. this is for giving up return; } // if (respond_to_depository_found(MOVING_TO_DEPO)) return; // if (respond_to_depository_found(SKIPPING_DEPO)) return; } }
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; 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 wait1Msec(delayAutoStartValue); // INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE 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 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 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] < 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; break; 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; 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 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; default: // Not a valid autonomous action break; } // All Done, time to stop all tasks StopAllTasks(); } // End Main Bracket