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
