task main() { Task_Kill(displayDiagnostics); Display_Clear(); initializeGlobalVariables(); // Defined in "initialize.h", this intializes all struct members. const int finish_delay = 4*1000; // MAGIC_NUM: milliseconds to delay when program ends. // TODO: Figure out why the delay is nowhere near this. TFileHandle IO_handle; TFileIOResult IO_result; const string filename = "_reset_pods.txt"; const string filename_temp = "_reset_pods_tmp.txt"; // temp makes the filename too long?? int file_size = 0; short rotation[POD_NUM] = {0,0,0,0}; // Read in all the values of the pods from a text file. OpenRead(IO_handle, IO_result, filename, file_size); // TODO: Add more error handling. if (IO_result==ioRsltSuccess) { nxtDisplayTextLine(0, "Read from file"); nxtDisplayTextLine(1, "\"_reset_pods.txt\""); } else if (IO_result==ioRsltFileNotFound) { OpenRead(IO_handle, IO_result, filename_temp, file_size); // TODO: Add more error handling. if (IO_result==ioRsltSuccess) { nxtDisplayTextLine(0, "Read from file"); nxtDisplayTextLine(1, "\"_pods_tmp.txt\""); } else if ( (IO_result==ioRsltFileNotFound) || (IO_result==ioRsltNoSpace) || (IO_result==ioRsltNoMoreFiles) ) { nxtDisplayTextLine(2, "No data file."); nxtDisplayTextLine(3, "Terminating."); Time_Wait(finish_delay); return; // The only way to break out of this function? } } else if ((IO_result==ioRsltNoSpace)||(IO_result==ioRsltNoMoreFiles)) { nxtDisplayTextLine(2, "No data file."); nxtDisplayTextLine(3, "Terminating."); Time_Wait(finish_delay); return; // The only way to break out of this function? } for (int i=POD_FR; i<(int)POD_NUM; i++) { ReadShort(IO_handle, IO_result, rotation[i]); } Close(IO_handle, IO_result); nxtDisplayTextLine(3, "FL:%5d FR:%5d", rotation[POD_FL], rotation[POD_FR]); nxtDisplayTextLine(4, "BL:%5d BR:%5d", rotation[POD_BL], rotation[POD_BR]); nxtDisplayCenteredTextLine(6, "Press [ENTER] to"); nxtDisplayCenteredTextLine(7, "end program."); do { Buttons_UpdateData(); Time_Wait(100); // MAGIC_NUM: Arbitrary LCD refresh delay. } while (Buttons_Released(NXT_BUTTON_YES)==false); }
task main() { initializeGlobalVariables(); servoPrep(); Task_Spawn(t_liftEncoder); Joystick_WaitForStart(); while (true) { Joystick_UpdateData(); // driving segment // for full power, press start controller 1 if (Joystick_Button(BUTTON_START)) { isFullPower = true; } // for fine tuning, press back on controller 1 else if (Joystick_Button(BUTTON_BACK)) { isFullPower = false; } // full power segment if(isFullPower) { drivePowerL = Joystick_GenericInput(JOYSTICK_L, AXIS_Y, CONTROLLER_1); drivePowerR = Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_1); } // fine tuning section else { drivePowerL = Joystick_GenericInput(JOYSTICK_L, AXIS_Y, CONTROLLER_1) / 2; drivePowerR = Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_1) / 2; } // the Left Joystick on Controller 1 controls the left wheel motor[leftWheel] = drivePowerL; // the Right Joystick on Controller 1 controls the right wheel motor[rightWheel] = drivePowerR; // for lift // Holding RB on Controller 1 raises the lift if(Joystick_Button(BUTTON_RB, CONTROLLER_1)) { isLift = true; Task_Spawn(t_raiseLift); } // Releasing RB on Controller 1 stops the lift else if(Joystick_ButtonReleased(BUTTON_RB, CONTROLLER_1)) { isLift = false; Task_Kill(t_raiseLift); Task_Spawn(t_stopLift); } // Holding LB on Controller 1 lowers the lift else if(Joystick_Button(BUTTON_LB, CONTROLLER_1)) { isLift = true; Task_Spawn(t_lowerLift); } // Releasing LB on Controller 1 stops the lift else if(Joystick_ButtonReleased(BUTTON_LB, CONTROLLER_1)) { isLift = false; Task_Kill(t_lowerLift); Task_Spawn(t_stopLift); } // pressing B on Controller 1 puts lift in low goal pos /* else if(Joystick_Button(BUTTON_B, CONTROLLER_1) && !isLift) { Task_Spawn(t_raiseLiftLow); goalPos = goalPosLow; } pressing X on Controller 1 puts lift in middle goal pos else if(Joystick_Button(BUTTON_X, CONTROLLER_1) && !isLift) { Task_Spawn(t_raiseLiftMiddle); goalPos = goalPosMid; } // pressing Y on Controller 1 puts lift in high goal pos else if(Joystick_Button(BUTTON_Y, CONTROLLER_1) && !isLift) { Task_Spawn(t_raiseLiftHigh); goalPos = goalPosHigh; } // pressing A on Controller 1 lowers lift to ground else if(Joystick_Button(BUTTON_A, CONTROLLER_1) && !isLift && goalPos == goalPosLow) { Task_Spawn(t_lowerLiftLow); } else if(Joystick_Button(BUTTON_A, CONTROLLER_1) && !isLift && goalPos == goalPosMid) { Task_Spawn(t_lowerLiftMiddle); } else if(Joystick_Button(BUTTON_A,CONTROLLER_1) && !isLift && goalPos == goalPosHigh) { Task_Spawn(t_lowerLiftHigh); } // make a kill lift button */ // loop controlling the pickup // Holding RT on Controller 1 picks up balls if(Joystick_Button(BUTTON_RT, CONTROLLER_1)) { Task_Kill(t_reversePickup); Task_Spawn(t_startPickup); } // Releasing RT on Controller 1 stops the pickup if(Joystick_ButtonReleased(BUTTON_RT, CONTROLLER_1)) { Task_Kill(t_startPickup); Task_Spawn(t_stopPickup); } // Holding LT on Controller 1 releases balls from the pickup else if(Joystick_Button(BUTTON_LT, CONTROLLER_1)) { Task_Kill(t_startPickup); Task_Spawn(t_reversePickup); } // Releasing LT on Controller 1 stops the pickup else if(Joystick_ButtonReleased(BUTTON_LT, CONTROLLER_1)) { Task_Kill(t_reversePickup); Task_Spawn(t_stopPickup); } // for clamp // Pressing Button A on Controller 2 drops the clamp if(Joystick_Button(BUTTON_A, CONTROLLER_2)) { Task_Kill(t_raiseClamp); Task_Spawn(t_dropClamp); } // Pressing Button B on Controller 2 raises the clamp if(Joystick_Button(BUTTON_B, CONTROLLER_2)) { Task_Kill(t_dropClamp); Task_Spawn(t_raiseClamp); } // for the drop servo // Pressing X on Controller 2 drops the balls from the basket if(Joystick_Button(BUTTON_X, CONTROLLER_2)) { Task_Kill(t_resetDrop); Task_Spawn(t_dropBall); } // Pressing Y on Controller 2 resets the drop servo so balls doon't fall out of the basket if(Joystick_Button(BUTTON_Y, CONTROLLER_2)) { Task_Kill(t_dropBall); Task_Spawn(t_resetDrop); } wait1Msec(1); } }
task Display() { typedef enum DisplayMode { DISP_FCS, // Default FCS screen. DISP_ENCODERS, // Raw encoder values. DISP_LIFT, // PID, status, mode DISP_HOPPER, // Maths variables. DISP_SENSORS, // Might need to split this into two screens. DISP_JOYSTICKS, // For convenience. TODO: Add buttons, D-pad, etc.? DISP_NUM }; Task_Spawn(displayDiagnostics); // Explicit here: this is only spawned when buttons are pressed. DisplayMode isMode = DISP_FCS; // We don't need to wait for start. ;) while (true) { Buttons_UpdateData(); switch (isMode) { case DISP_FCS : break; case DISP_ENCODERS : nxtDisplayTextLine(0, "Lift : %+6i", lift_pos); nxtDisplayTextLine(1, "Mtr L: %+6i", Motor_GetEncoder(encoder_L)); nxtDisplayTextLine(2, "Mtr R: %+6i", Motor_GetEncoder(encoder_R)); break; case DISP_SENSORS : nxtDisplayTextLine(0, "Angle: %3i", heading); nxtDisplayTextLine(1, "IR A : %3i", IR_A); nxtDisplayTextLine(2, "IR B : %3i", IR_B); nxtDisplayTextLine(3, "IR C : %3i", IR_C); nxtDisplayTextLine(4, "IR D : %3i", IR_D); nxtDisplayTextLine(5, "IR E : %3i", IR_E); break; case DISP_LIFT : string lift_manual_str; string lift_status_str; if (is_lift_manual) { lift_manual_str = "MANUAL"; } else { lift_manual_str = "PID"; } if (isDown) { lift_status_str = "DOWN"; } else { lift_status_str = "UP"; } nxtDisplayCenteredTextLine(0, "Lift-%s-%s", lift_manual_str, lift_status_str); nxtDisplayTextLine(1, "Pos: %+6i", lift_pos); nxtDisplayTextLine(2, "Tgt: %+6i", lift_target); nxtDisplayTextLine(3, "Pwr: %+3.3f", power_lift); nxtDisplayCenteredTextLine(4, "%+4i %+4i %+4i", term_P_lift, term_I_lift, term_D_lift); break; case DISP_HOPPER : nxtDisplayTextLine(0, "XYZ %+2.1f %+2.1f %3i", hopper_x_pos, hopper_y_pos, hopper_z_pos); nxtDisplayTextLine(1, "XYZ %+2.1f %+2.1f %3i", hopper_x_target, hopper_y_target, hopper_z_target); nxtDisplayTextLine(2, "k,i %+4i %+4i", hopper_theta, hopper_phi); nxtDisplayTextLine(3, "r,h %3.1f %3.1f", hopper_r, hopper_h); nxtDisplayTextLine(4, "encdr: %+6d", hopper_pos); nxtDisplayTextLine(5, "tgt : %+6d", hopper_target); break; case DISP_JOYSTICKS : nxtDisplayCenteredTextLine(0, "--Driver I:--"); nxtDisplayCenteredTextLine(1, "LX:%4i RX:%4i", joystick.joy1_x1, joystick.joy1_x2); nxtDisplayCenteredTextLine(2, "LY:%4i RY:%4i", joystick.joy1_y1, joystick.joy1_y2); nxtDisplayCenteredTextLine(4, "--Driver II:--"); nxtDisplayCenteredTextLine(5, "LX:%4i RX:%4i", joystick.joy2_x1, joystick.joy2_x2); nxtDisplayCenteredTextLine(6, "LY:%4i RY:%4i", joystick.joy2_y1, joystick.joy2_y2); break; default : nxtDisplayCenteredTextLine(2, "Debug info"); nxtDisplayCenteredTextLine(3, "for this screen"); nxtDisplayCenteredTextLine(4, "is not currently"); nxtDisplayCenteredTextLine(5, "available."); break; } if (Buttons_Released(NXT_BUTTON_L)==true) { Display_Clear(); isMode = (DisplayMode)((isMode+DISP_NUM-1)%DISP_NUM); if (isMode==DISP_FCS) { Task_Spawn(displayDiagnostics); } else { Task_Kill(displayDiagnostics); } } if (Buttons_Released(NXT_BUTTON_R)==true) { Display_Clear(); isMode = (DisplayMode)((isMode+DISP_NUM+1)%DISP_NUM); if (isMode==DISP_FCS) { Task_Spawn(displayDiagnostics); } else { Task_Kill(displayDiagnostics); } } Time_Wait(50); // MAGIC_NUM: Prevents the LCD from updating itself to death. } }