task Hopper()
{
Joystick_WaitForStart();
while (true) {
//if (servo_turntable_pos<pos_servo_turntable_BL) {
// servo_turntable_pos = pos_servo_turntable_BL;
//} else if (servo_turntable_pos>pos_servo_turntable_BR) {
// servo_turntable_pos = pos_servo_turntable_BR;
//}
if (lift_pos<pos_hopper_safety_down) {
servo_turntable_pos = pos_servo_turntable_F;
}
if (hopper_pos<pos_hopper_safety_side) {
servo_turntable_pos = pos_servo_turntable_F;
}
servo_turntable_pos = pos_servo_turntable_F;
Servo_SetPosition(servo_turntable, (int)round(servo_turntable_pos));
if (lift_pos < pos_hopper_safety_down) {
if (hopper_pos > (pos_servo_hopper_down+3)) {
int lift_target_buf = lift_target;
int hopper_target_buf = hopper_target;
while (hopper_pos > (pos_servo_hopper_down+3)) {
lift_target = pos_hopper_safety_down;
is_lift_manual = false;
hopper_pos = encoderToHopper(encoder_hopper);
hopper_target = pos_servo_hopper_down;
Servo_SetPosition(servo_hopper_A, hopper_target);
Servo_SetPosition(servo_hopper_B, hopper_target);
Time_Wait(5);
}
lift_target = lift_target_buf;
hopper_target = hopper_target_buf;
}
}
if (hopper_target < pos_servo_hopper_down) {
hopper_target = pos_servo_hopper_down;
} else if (hopper_target > pos_servo_hopper_goal) {
hopper_target = pos_servo_hopper_goal;
}
Servo_SetPosition(servo_hopper_A, hopper_target);
Servo_SetPosition(servo_hopper_B, hopper_target);
Time_Wait(2);
}
}
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() {
servoPrep();
Joystick_WaitForStart();
disableDiagnosticsDisplay();
while(true) {
encoderPrep();
startTrackers();
driveForward(310.0);
turnRight(85.0);
if(irDetected == true) {
irPos2 = true;
turnRight(40.0);
irDetected = false;
driveBackward(25.0);
if(irDetected == true) {
driveBackward(15.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(105.0);
turnRight(85.0);
driveBackward(300.0);
}
else {
irPos1 = true;
driveBackward(110.0);
turnRight(46.0);
driveBackward(135.0);
turnRight(3.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(130.0);
turnRight(82.0);
driveBackward(300.0);
}
}
else {
driveForward(115.0);
// ir loop
if(irDetected == true) {
irPos3 = true;
driveBackward(15.0);
raiseLift(750.0);
dropBallCenter();
wait1Msec(300);
servo[centerServo] = endPosCenter - 30;
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
lowerLift(1000.0);
driveForward(120.0);
turnRight(85.0);
driveBackward(300.0, 60);
/*turnRight(43.0);
driveBackward(250.0);
motor[rightWheel] = -20;
motor[leftWheel] = -20;
wait1Msec(200);
motor[rightWheel] = 0;
motor[leftWheel] = 0;
dropClamp();
driveForward(10.0);
raiseLift(700.0);
dropBallGoal();
resetDropGoal();*/
break;
}
else {
driveBackward(220.0);
turnRight(85.0);
driveBackward(100.0);
}
}
break;
}
}
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 PID()
{
const float kP_up = 0.082;
const float kI_up = 0.017;
const float kD_up = 0.00;
const float kP_down = 0.002;
const float kI_down = 0.005;
const float kD_down = 0.00;
const float I_term_decay_rate = 0.87;
const int I_term_threshold = 500;
int timer_loop = 0;
Time_ClearTimer(timer_loop);
int dt = Time_GetTime(timer_loop);
lift_pos = Motor_GetEncoder(encoder_lift);
float error = 0.0;
float error_prev = 0.0;
float error_sum = 0.0;
float error_rate = 0.0;
Joystick_WaitForStart();
Time_ClearTimer(timer_loop);
while (true) {
dt = Time_GetTime(timer_loop);
Time_ClearTimer(timer_loop);
error_prev = error;
lift_pos = Motor_GetEncoder(encoder_lift);
if (is_lift_manual == false) {
if (lift_target < pos_lift_bottom) {
lift_target = pos_lift_bottom;
}
if (lift_target > pos_lift_top) {
lift_target = pos_lift_top;
}
error = lift_target - lift_pos;
if (error > 0) {
isDown = false;
} else {
isDown = true;
}
error_sum *= I_term_decay_rate;
if (error < I_term_threshold) {
error_sum += error * (float)dt;
}
error_rate = (error - error_prev) / (float)dt;
term_P_lift = error;
term_I_lift = error_sum;
term_D_lift = error_rate;
switch (isDown) {
case true :
term_P_lift *= kP_down;
term_I_lift *= kI_down;
term_D_lift *= kD_down;
break;
case false :
term_P_lift *= kP_up;
term_I_lift *= kI_up;
term_D_lift *= kD_up;
break;
}
power_lift = term_P_lift + term_I_lift + term_D_lift;
} else {
lift_target = lift_pos;
power_lift = power_lift_temp;
}
if (abs(power_lift)<20) {
power_lift = 0;
}
if (isLiftFrozen) {
power_lift = 0;
}
if (isReset == false) {
if (power_lift>0 && lift_pos>pos_lift_top) {
power_lift = 0;
} else if (power_lift<0 && lift_pos<pos_lift_bottom) {
power_lift = 0;
}
} else {
Motor_ResetEncoder(encoder_lift);
}
Motor_SetPower(power_lift, motor_lift_A);
Motor_SetPower(power_lift, motor_lift_B);
Time_Wait(2);
}
}
