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