task main()
{
// These will be used every loop, and are declared
// here to save from declaring them every loop.
int powerL = 0;
int powerR = 0;
//// Not implemented yet. Will implement when adding ring code.
//MotorState isMotorStateL = MOTOR_JOYSTICK;
//MotorState isMotorStateR = MOTOR_JOYSTICK;
waitForStart();
initializeRobot();
// At "max capacity", each loop should do 8 checks and
// 3 assignments: 2 x (2 joysticks + buttons + 1 D-pad).
// The order is:
// (D-pad)
// Joysticks
// Buttons
// Default arguments are never passed if not needed (to optimize).
// Using a `for` loop (instead of `while`) is more intuitive,
// flexible, and makes code more readable (e.g. for indexing).
for (int i=0; ; i++) // `int i` is used in many included headers.
{
Joystick_UpdateData();
// These should be zeroed after every loop, so if there
// isn't any input, the motors will have no power.
powerL = 0;
powerR = 0;
powerLift = 0;
// CONTROLLER 2 INPUT:==================================================||>
// JOYSTICKS INPUT:--------------------------------------------------|>
// Input from the two joysticks will control the lift at
// different speeds (fast for R, slow for L). These will
// not override the input from the primary driver.
// This is because the input from the primary driver is
// processed last, therefore getting "the last say".
// The signals are only processed if it is above a threshold.
if ( abs(Joystick_Joystick(JOYSTICK_L, AXIS_Y, CONTROLLER_2))
>g_JoystickThreshold )
{
isLiftState = LIFT_JOYSTICK;
powerLift =
Math_ToLogarithmic
(Joystick_Joystick(JOYSTICK_L, AXIS_Y, CONTROLLER_2));
}
if ( abs(Joystick_Joystick(JOYSTICK_R, AXIS_Y, CONTROLLER_2))
>g_JoystickThreshold )
{
isLiftState = LIFT_JOYSTICK;
powerLift =
Math_ToLogarithmic
(Joystick_Joystick(JOYSTICK_R, AXIS_Y, CONTROLLER_2)
/g_FineTuneFactor);
}
// BUTTONS INPUT:--------------------------------------------------|>
// This includes servo controls and the ramp release confirm.
// Uncomment the next line and comment the one after if masking.
// For an explanation of masking the buttons' input,
// see the comments accompanying CONTROLLER_1's buttons.
//if ( (g_ControllerMaskB & joystick.joy2_Buttons) != false )
if ( joystick.joy2_Buttons != false )
{
if ( Joystick_Button(BUTTON_LT, CONTROLLER_2)==true )
{
Servo_Rotate(servo_IR, g_IRServoLowered);
}
if ( Joystick_Button(BUTTON_LB, CONTROLLER_2)==true )
{
Servo_Rotate(servo_IR, g_IRServoExtended);
}
if ( Joystick_Button(BUTTON_RT, CONTROLLER_2)==true )
{
Servo_Rotate(servo_IR, g_clawServoFolded);
}
if ( Joystick_Button(BUTTON_RB, CONTROLLER_2)==true )
{
Servo_Rotate(servo_IR, g_clawServoFolded);
}
// Both controllers need to press START to deploy ramp.
// The code is in CONTROLLER_1's buttons code block.
if ( Joystick_Button(BUTTON_BACK, CONTROLLER_2)==true )
{
Servo_Rotate(servo_ramp, g_rampServoHold);
}
}
// CONTROLLER 1 INPUT:==================================================||>
// D-PAD INPUT:--------------------------------------------------|>
// Only if D-pad is pressed, test for direction.
// Controls lift and has two unimplemented functions
// for taking rings off/putting rings on.
if ( Joystick_Direction() != DIRECTION_NONE )
{
switch ( Joystick_Direction() )
{
// Operate lift at full power if F/B.
case DIRECTION_F:
isLiftState = LIFT_JOYSTICK;
powerLift = g_FullLiftPower;
break;
case DIRECTION_B:
isLiftState = LIFT_JOYSTICK;
powerLift = (-1)*g_FullLiftPower;
break;
case DIRECTION_L:
sub_PutRingOn();
break;
case DIRECTION_R:
sub_TakeRingOff();
break;
}
}
// JOYSTICKS INPUT:--------------------------------------------------|>
// Controls most of the driving. We are using two separate
// checks, because combining them into one check will execute
// both even if only one joystick is pressed. The linking of
// both checks (disjunction) checks both inputs anyways.
// The signals are only processed if it is above a threshold.
if ( abs(Joystick_Joystick(JOYSTICK_L, AXIS_Y)) > g_JoystickThreshold )
{
powerL = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_L, AXIS_Y));
}
if ( abs(Joystick_Joystick(JOYSTICK_R, AXIS_Y)) > g_JoystickThreshold )
{
powerR = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_R, AXIS_Y));
}
// I don't actually have any code for real arcade driving :P
// BUTTONS INPUT:--------------------------------------------------|>
// Everything other than the buttons used might be masked to
// (possibly) increase speed.
// Reasoning: `&` compares every bit, so we might as well mask,
// in case something irrelevant is pressed.
// A `0` value means no buttons (that we test for) are pressed.
// Directly using the struct since this is the only time we
// use it, and the code is meant to be low-level anyways.
// Uncomment the next line and comment the one after if masking.
// For an explanation of masking the buttons' input,
// see the comments accompanying CONTROLLER_1's buttons.
//if ( (g_ControllerMaskA & joystick.joy1_Buttons) != false )
if ( joystick.joy1_Buttons != false )
{
// Buttons Y/B/A/X will control lift height.
if ( Joystick_Button(BUTTON_Y)==true )
{
isLiftState = LIFT_TOP;
}
if ( Joystick_Button(BUTTON_B)==true )
{
isLiftState = LIFT_MIDDLE;
}
if ( Joystick_Button(BUTTON_A)==true )
{
isLiftState = LIFT_BOTTOM;
}
if ( Joystick_Button(BUTTON_X)==true )
{
isLiftState = LIFT_FETCH;
}
// Buttons LT/RT fine-tune the lift.
if ( Joystick_Button(BUTTON_RT)==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift = g_FullLiftPower/g_FineTuneFactor;
}
if ( Joystick_Button(BUTTON_LT)==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift = (-1)*g_FullLiftPower/g_FineTuneFactor;
}
// If LB/RB is pressed, fine-tune the motors.
if ( (Joystick_Button(BUTTON_LB)||
Joystick_Button(BUTTON_RB)) ==true )
{
powerL /= g_FineTuneFactor;
powerR /= g_FineTuneFactor;
}
// Both controllers need to press START to deploy ramp.
if ( (Joystick_Button(BUTTON_START)&&
Joystick_Button(BUTTON_START, CONTROLLER_2))==true )
{
Servo_Rotate(servo_ramp, g_rampServoDeployed);
}
if ( Joystick_Button(BUTTON_BACK)==true )
{
Servo_Rotate(servo_ramp, g_rampServoHold);
}
}
// FINAL PROCESSING:==================================================||>
// After preliminary processing of the controller data,
// the actual motor/servo assignments happen here.
switch (isLiftState)
{
case LIFT_BOTTOM:
sub_LiftToHeight(g_BottomLiftAngle);
break;
case LIFT_MIDDLE:
sub_LiftToHeight(g_MiddleLiftAngle);
break;
case LIFT_TOP:
sub_LiftToHeight(g_TopLiftAngle);
break;
case LIFT_FETCH:
sub_LiftToHeight(g_FetchLiftAngle);
}
Motor_SetPower(motor_L, powerL);
Motor_SetPower(motor_R, powerR);
Motor_SetPower(motor_lift, powerLift);
}
}
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);
}
}
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()
{
// These will be used later and are declared here to save from having to
// declare them every single loop.
int powerL = 0;
int powerR = 0;
int powerPopcorn = 0;
MotorState isMotorStateL = MOTOR_JOYSTICK;
MotorState isMotorStateR = MOTOR_JOYSTICK;
waitForStart();
initializeRobot();
while (true)
{
// Currently does (at least) 7 checks and 3 assignments per loop.
Joystick_UpdateData();
// These should be zeroed after every loop. In the case that there
// isn't input, the motors won't keep moving at the last speed it had.
powerL = 0;
powerR = 0;
powerLift = 0;
powerPopcorn = 0;
// POPCORN!!! (This comes first, obviously.)
if ( Joystick_Button(BUTTON_B, CONTROLLER_2)==true )
{
powerPopcorn = g_FullDrivePower;
}
else if ( Joystick_Button(BUTTON_A, CONTROLLER_2)==true )
{
powerPopcorn = (-1)*g_FullDrivePower;
}
// See if a direction is being pressed, then test for the direction.
// This is inside an `if` statement to optimize speed (less checking).
// `JoystickController` arguments are not passed to increase speed.
// Input from CONTROLLER_2 will be used to control the lift in
// conjunction with CONTROLLER_1, but shouldn't override the driver,
// since driver #1's input is processed last.
// This is the code for CONTROLLER_2:
if ( abs(joystick.joy2_y1)>g_JoystickThreshold )
{
isLiftState = LIFT_JOYSTICK;
//powerLift = Math_ToLogarithmic(joystick.joy2_y1);
powerLift = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_L, AXIS_Y, CONTROLLER_2));
}
if ( ( Joystick_Button(BUTTON_LB, CONTROLLER_2) ||
Joystick_Button(BUTTON_RB, CONTROLLER_2)) ==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift /= g_FineTuneFactor;
}
// This is the code for CONTROLLER_1, along with two unimplemented
// functions for putting rings on and taking rings off.
if ( Joystick_Direction() != DIRECTION_NONE )
{
switch ( Joystick_Direction() )
{
// Operate lift at full power if F/B.
case DIRECTION_F:
isLiftState = LIFT_JOYSTICK;
powerLift = g_FullLiftPower;
break;
case DIRECTION_B:
isLiftState = LIFT_JOYSTICK;
powerLift = (-1)*g_FullLiftPower;
break;
case DIRECTION_L:
sub_PutRingOn();
break;
case DIRECTION_R:
StartTask(sub_TakeRingOff);
break;
}
}
// See if a button (not masked) is being pressed, then react.
// This is inside an `if` statement to optimize speed (less checking).
// The argument to this first `if` statement is a masked version
// of the "bitmap" of buttons directly from the controller.
// Everything other than the buttons used are masked off, to increase
// processing speed (possibly, just speculation). Reasoning:
// `&` compares all bits of the variables, so we might as well mask
// everything we won't need, in case something irrelevant is pressed.
// A `0` value means no buttons (that we are testing for) are pressed.
// Directly using the struct since this is the only possible time to
// use it, and this is very low-level anyways.
//if ( joystick.joy1_Buttons != false )
//if ( (g_ControllerMaskA & joystick.joy1_Buttons) != false )
{
// Buttons Y/B/A will control lift height.
if ( Joystick_Button(BUTTON_Y)==true )
{
isLiftState = LIFT_TOP;
}
if ( Joystick_Button(BUTTON_B)==true )
{
isLiftState = LIFT_MIDDLE;
}
if ( Joystick_Button(BUTTON_A)==true )
{
isLiftState = LIFT_BOTTOM;
}
// If only X is pressed, weigh the ring.
// If JOYR is pressed as well, deploy ramp.
if ( Joystick_Button(BUTTON_X)==true )
{
if ( Joystick_Button(BUTTON_JOYR) == true )
{
Servo_Rotate(servo_ramp, g_rampServoDeployed);
}
else
{
StartTask(sub_WeighRings);
}
}
// Buttons LT/RT will fine-tune the lift.
if ( Joystick_Button(BUTTON_RT)==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift = g_FullLiftPower/g_FineTuneFactor;
}
if ( Joystick_Button(BUTTON_LT)==true )
{
isLiftState = LIFT_JOYSTICK;
powerLift = (-1)*g_FullLiftPower/g_FineTuneFactor;
}
}
// L/R motor code. Only triggered when a joystick returns a
// value greater than the "drive" threshold (`global vars.h`).
// Logarithmic control probably won't be implemented anytime soon.
// Also need to stop using the `joystick` struct and switch to the
// encapsulated version (Joystick_Joystick(...)).
// Y-axis code:
if ( abs(Joystick_Joystick(JOYSTICK_L, AXIS_Y)) > g_JoystickThreshold ||
abs(Joystick_Joystick(JOYSTICK_R, AXIS_Y)) > g_JoystickThreshold )
{
powerL = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_L, AXIS_Y));
powerR = Math_ToLogarithmic(Joystick_Joystick(JOYSTICK_R, AXIS_Y));
}
// Last check: if LB/RB is pressed, fine-tune the power level.
if ( (Joystick_Button(BUTTON_LB)||Joystick_Button(BUTTON_RB)) ==true )
{
powerL /= g_FineTuneFactor;
powerR /= g_FineTuneFactor;
}
// CONTROLLER_2 has the same masking implementation as CONTROLLER_1.
// For a detailed explanation of the mechanism, see those comments.
// CONTROLLER_2 is only tested for button X (currently).
//if ( joystick.joy2_Buttons != false )
//if ( (g_ControllerMaskB & joystick.joy2_Buttons) != false )
{
// If X is pressed, the MOO shall be released!
if ( Joystick_Button(BUTTON_X, CONTROLLER_2)==true )
{
//StartTask(sub_MOO);
PlaySoundFile("moo.rso");
}
if ( Joystick_Button(BUTTON_Y, CONTROLLER_2)==true )
{
//StopTask(sub_MOO);
sub_CowsWithGuns();
}
}
switch (isLiftState)
{
case LIFT_BOTTOM:
sub_LiftToBottom();
break;
case LIFT_MIDDLE:
sub_LiftToMiddle();
break;
case LIFT_TOP:
sub_LiftToTop();
break;
}
// Flush the controller input buffer periodically (every 1/4 sec?)
Motor_SetPower(motor_L, powerL);
Motor_SetPower(motor_R, powerR);
Motor_SetPower(motor_lift, powerLift);
Motor_SetPower(motor_popcorn, powerPopcorn);
}
}
