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 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);
}
}
