예제 #1
0
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);
    }
}
예제 #2
0
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);



	}
}