task main() { typedef enum MotorDirection { DIRECTION_NONE = 0, DIRECTION_IN, DIRECTION_OUT }; typedef enum PickupPos { PICKUP_RETRACT = 0, PICKUP_LARGE, PICKUP_SMALL, PICKUP_KICK }; initializeGlobalVariables(); initializeRobotVariables(); MotorDirection pickup_I_direction = DIRECTION_NONE; MotorDirection pickup_I_direction_prev = DIRECTION_NONE; MotorDirection pickup_O_direction = DIRECTION_NONE; MotorDirection pickup_O_direction_prev = DIRECTION_NONE; MotorDirection clamp_direction = DIRECTION_NONE; PickupPos pickup_pos = PICKUP_LARGE; int servo_dump_pos = pos_servo_dump_closed; float power_L = 0.0; float power_R = 0.0; float power_pickup_I = 0.0; float power_pickup_O = 0.0; float power_clamp = 0.0; Task_Spawn(Gyro); Task_Spawn(PID); Task_Spawn(Display); Task_Spawn(Hopper); Joystick_WaitForStart(); while (true) { Joystick_UpdateData(); HTIRS2readAllACStrength(sensor_IR, IR_A, IR_B, IR_C, IR_D, IR_E); hopper_pos = encoderToHopper(Motor_GetEncoder(encoder_hopper)); power_L = Joystick_GenericInput(JOYSTICK_L, AXIS_Y); power_R = Joystick_GenericInput(JOYSTICK_R, AXIS_Y); power_lift_temp = Joystick_GenericInput(JOYSTICK_L, AXIS_Y, CONTROLLER_2); if (abs(power_lift_temp) > 5) { is_lift_manual = true; } else { is_lift_manual = false; } if (Joystick_Button(BUTTON_JOYL, CONTROLLER_2) == true) { isReset = true; } else { isReset = false; } //servo_turntable_pos -= 0.004 * Joystick_GenericInput(JOYSTICK_R, AXIS_X, CONTROLLER_2); if (Joystick_Button(BUTTON_JOYR, CONTROLLER_2) || Joystick_ButtonReleased(BUTTON_JOYR, CONTROLLER_2)) { servo_turntable_pos = pos_servo_turntable_F; hopper_target = pos_servo_hopper_down; } if (abs(Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_2))>0) { hopper_target = hopper_pos; hopper_target += 0.6 * Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_2); } hopper_r = sqrt(hopper_x_target*hopper_x_target + hopper_y_target*hopper_y_target); hopper_theta = atan2(hopper_y_target, hopper_x_target); if (Joystick_ButtonPressed(BUTTON_B)) { pickup_I_direction_prev = pickup_I_direction; pickup_I_direction = DIRECTION_OUT; } if (Joystick_ButtonPressed(BUTTON_A)) { pickup_I_direction_prev = pickup_I_direction; pickup_I_direction = DIRECTION_IN; } if (Joystick_ButtonReleased(BUTTON_B)) { pickup_I_direction = pickup_I_direction_prev; } if (Joystick_ButtonReleased(BUTTON_A)) { pickup_I_direction = pickup_I_direction_prev; } if (Joystick_ButtonPressed(BUTTON_Y)) { pickup_O_direction_prev = pickup_O_direction; pickup_O_direction = DIRECTION_OUT; } if (Joystick_ButtonPressed(BUTTON_X)) { pickup_O_direction_prev = pickup_O_direction; pickup_O_direction = DIRECTION_IN; } if (Joystick_ButtonReleased(BUTTON_Y)) { pickup_O_direction = pickup_O_direction_prev; } if (Joystick_ButtonReleased(BUTTON_X)) { pickup_O_direction = pickup_O_direction_prev; } if (Joystick_ButtonPressed(BUTTON_RB)) { switch (pickup_O_direction) { case DIRECTION_NONE : case DIRECTION_OUT : pickup_O_direction = DIRECTION_IN; pickup_I_direction = DIRECTION_IN; break; case DIRECTION_IN : pickup_O_direction = DIRECTION_NONE; pickup_I_direction = DIRECTION_NONE; break; } } if (Joystick_ButtonPressed(BUTTON_RT)) { pickup_O_direction_prev = pickup_O_direction; pickup_I_direction_prev = pickup_I_direction; pickup_O_direction = DIRECTION_OUT; pickup_I_direction = DIRECTION_OUT; } if (Joystick_ButtonReleased(BUTTON_RT)) { pickup_O_direction = pickup_O_direction_prev; pickup_I_direction = pickup_I_direction_prev; } if (Joystick_DirectionPressed(DIRECTION_R)) { pickup_pos = PICKUP_RETRACT; } else if (Joystick_DirectionPressed(DIRECTION_F)) { pickup_pos = PICKUP_KICK; } else if (Joystick_DirectionPressed(DIRECTION_L)) { pickup_pos = PICKUP_LARGE; } else if (Joystick_DirectionPressed(DIRECTION_B)) { pickup_pos = PICKUP_SMALL; } if (Joystick_Button(BUTTON_LB)) { clamp_direction = DIRECTION_OUT; } else if (Joystick_Button(BUTTON_LT)) { clamp_direction = DIRECTION_IN; } else { clamp_direction = DIRECTION_NONE; } if (Joystick_Button(BUTTON_RB, CONTROLLER_2)) { servo_dump_pos = pos_servo_dump_open_small; } else if (Joystick_Button(BUTTON_RT, CONTROLLER_2)) { servo_dump_pos = pos_servo_dump_open_large; } else { servo_dump_pos = pos_servo_dump_closed; } if (pickup_I_direction==DIRECTION_IN && lift_pos<pos_dump_safety) { servo_dump_pos = pos_servo_dump_open_feed; } if (Joystick_Button(BUTTON_LB, CONTROLLER_2)) { clamp_direction = DIRECTION_IN; } if (Joystick_Button(BUTTON_LT, CONTROLLER_2)) { clamp_direction = DIRECTION_OUT; } if (Joystick_ButtonPressed(BUTTON_X, CONTROLLER_2)) { lift_target = pos_lift_bottom; is_lift_manual = false; hopper_target = pos_servo_hopper_down; servo_turntable_pos = pos_servo_turntable_F; isLiftFrozen = true; servo_dump_pos = pos_servo_dump_closed; } else if (Joystick_ButtonPressed(BUTTON_A, CONTROLLER_2)) { lift_target = pos_lift_low; is_lift_manual = false; hopper_target = pos_servo_hopper_goal; servo_turntable_pos = pos_servo_turntable_F; isHopperFrozen = true; servo_dump_pos = pos_servo_dump_closed; } else if (Joystick_ButtonPressed(BUTTON_B, CONTROLLER_2)) { lift_target = pos_lift_medium; is_lift_manual = false; hopper_target = pos_servo_hopper_goal; servo_turntable_pos = pos_servo_turntable_F; isHopperFrozen = true; servo_dump_pos = pos_servo_dump_closed; } else if (Joystick_ButtonPressed(BUTTON_Y, CONTROLLER_2)) { lift_target = pos_lift_high; is_lift_manual = false; hopper_target = pos_servo_hopper_goal; servo_turntable_pos = pos_servo_turntable_F; isHopperFrozen = true; servo_dump_pos = pos_servo_dump_closed; } if (Joystick_ButtonPressed(BUTTON_START)) { Servo_SetPosition(servo_hopper_A, pos_servo_hopper_goal); Servo_SetPosition(servo_hopper_B, pos_servo_hopper_goal); if (abs(hopper_target-pos_servo_hopper_center)<3) { hopper_target = pos_servo_hopper_down; } else { hopper_target = pos_servo_hopper_center; } } if (Joystick_ButtonPressed(BUTTON_BACK)) { Servo_SetPosition(servo_hopper_A, pos_servo_hopper_center); Servo_SetPosition(servo_hopper_B, pos_servo_hopper_center); if (abs(hopper_target-pos_servo_hopper_goal)<3) { hopper_target = pos_servo_hopper_down; } else { hopper_target = pos_servo_hopper_goal; } } if (isLiftFrozen && hopper_pos < pos_servo_hopper_up) { isLiftFrozen = false; } if (isHopperFrozen) { hopper_target = pos_servo_hopper_up; if (abs(lift_pos-lift_target)<300) { hopper_target = pos_servo_hopper_goal; isHopperFrozen = false; } } switch (pickup_I_direction) { case DIRECTION_NONE : power_pickup_I = 0; break; case DIRECTION_IN : power_pickup_I = 100; break; case DIRECTION_OUT : power_pickup_I = -100; break; } switch (pickup_O_direction) { case DIRECTION_NONE : power_pickup_O = 0; break; case DIRECTION_IN : power_pickup_O = 100; break; case DIRECTION_OUT : power_pickup_O = -100; break; } switch (clamp_direction) { case DIRECTION_NONE : power_clamp = 0; break; case DIRECTION_IN : power_clamp = 100; break; case DIRECTION_OUT : power_clamp = -100; break; } Motor_SetPower(power_L, motor_LT); Motor_SetPower(power_L, motor_LB); Motor_SetPower(power_R, motor_RT); Motor_SetPower(power_R, motor_RB); Motor_SetPower(power_pickup_I, motor_pickup_I); Motor_SetPower(power_pickup_O, motor_pickup_O); Motor_SetPower(power_clamp, motor_clamp_L); Motor_SetPower(power_clamp, motor_clamp_R); Servo_SetPosition(servo_dump, servo_dump_pos); // NOTE: Hopper and turntable servos should be set in the "Hopper" task. switch (pickup_pos) { case PICKUP_RETRACT : Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_retract); Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_retract); break; case PICKUP_LARGE : Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_large); Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_large); break; case PICKUP_SMALL : Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_small); Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_small); break; case PICKUP_KICK : Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_kick); Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_kick); break; } Time_Wait(5); } }
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. } }