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.
}
}
