#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Motor,  mtr_S1_C1_1,     MOTOR_RIGHT,   tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     MOTOR_LEFT,    tmotorNormal, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C2_1,    SERVO_ARM,            tServoNormal)
#pragma config(Servo,  srvo_S1_C2_2,    SERVO_CLAW,           tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//#pragma config(Servo,  servo1,          SERVO_ARM,            tServoNormal)
//#pragma config(Servo,  servo2,          SERVO_CLAW,           tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// ORTOP MODULAR ROBOT
//
// Right and left joysticks drive the robot like a tank.
// Tilting the tophat forward and back move the arm forward and back.
// Pressing buttons 2 and 4 open and close the claw.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

// claw positions that represent open and closed (gripping cargo)
#define CLAW_OPEN          255
#define CLAW_CLOSED       200
//#define CLAW_OPEN          200
//#define CLAW_CLOSED        120

#define BTN_CLAW_OPEN        2
#define BTN_CLAW_CLOSED      4

// Various predefined arm servo positions
#define ARM_HOME           110   // Initial (and storage) position. Fully tucked back against the robot.
#define ARM_HIGHEST        155   // Position where the claw is as high as possible off the ground.
#define ARM_LOWEST         220   // Position where the claw is as low as it can get.
#define ARM_INCREMENT       10   // Amount we move the arm on each press of the tophat.

#define POSITIVE_DEADZONE   10   // Joystick values between the positive and
#define NEGATIVE_DEADZONE  -10   // negative deadzone values are treated as 0

// The tophat tilts in 8 directions returning numbers between 0 and 7. When idle it returns -1.
#define TOPHAT_IDLE         -1
#define TOPHAT_NORTH         0
#define TOPHAT_SOUTH         4

bool inTopHat;   // Flag to prevent one button press from moving the arm multiple times.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// InitializeRobot
//
// Code to get robot off on the right foot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void InitializeRobot()
{
    // make sure claw is in open position, arm is retracted
    servo(SERVO_CLAW) = CLAW_OPEN;
    servo(SERVO_ARM) = ARM_HOME;
    inTopHat = false;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// HandleDriveJoysticks
//
// The Y axis of the right joystick controls the right motor.
// The Y axis of the left joystick controls the left motor.
//
// The robot drives like a tank:
//    Both joysticks forward moves the robot forward.
//    Both joysticks backward moves the robot backward.
//    One joystick forward and the other backward spins the robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleDriveJoysticks()
{
    int jLeft, jRight;

    // Joystick values range from -128 to 127 but the motors only understand -100 to 100.
    // We need to scale the joystick values to be the proper range for the motors.

    jLeft = (joystick.joy1_y2 * 100) / 128;
    jRight = (joystick.joy1_y1 * 100) / 128;

    // Joysticks do not always return to 0 when you let go of them. There tends to be some "flutter"
    // and stickyness near zero. So, we'll make all values very close to zero be zero for both joysticks.

    if (jLeft > NEGATIVE_DEADZONE && jLeft < POSITIVE_DEADZONE)
        jLeft = 0;

    if (jRight > NEGATIVE_DEADZONE && jRight < POSITIVE_DEADZONE)
        jRight = 0;

   	// Set the motors to the adjusted joystick values

    motor[MOTOR_LEFT] = jLeft;
    motor[MOTOR_RIGHT] = jRight;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// HandleClawButtons
//
// See if either of the buttons that move the claw are pressed and do the right thing.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleClawButtons()
{
    if (joy1Btn(BTN_CLAW_CLOSED))
        servo(SERVO_CLAW) = CLAW_CLOSED;
    else if (joy1Btn(BTN_CLAW_OPEN))
        servo(SERVO_CLAW) = CLAW_OPEN;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// HandleArmTophat
//
// See if we need to move the arm again.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleArmTophat()
{
    // Only move the arm if the tophat is pressed AND we haven't already done something about it.

    if (joystick.joy1_TopHat != TOPHAT_IDLE && !inTopHat)
    {
        if (joystick.joy1_TopHat == TOPHAT_NORTH)
        {
            // Move the arm farther forward if we can
            if(servo(SERVO_ARM) <= ARM_LOWEST - ARM_INCREMENT)
                servo(SERVO_ARM) += ARM_INCREMENT;

            inTopHat = true;   // Don't move the arm again until the tophat is released.
        }

        if (joystick.joy1_TopHat == TOPHAT_SOUTH)
        {
            // Move the arm farther backwards if we can
            if(servo(SERVO_ARM) >= ARM_HIGHEST + ARM_INCREMENT)
                servo(SERVO_ARM) -= ARM_INCREMENT;

            inTopHat = true;   // Don't move the arm again until the tophat is released
        }
    }
    else if (joystick.joy1_TopHat == TOPHAT_IDLE && inTopHat)
    {
        // We've finished handling one press of the tophat.
        // Next time the tophat isn't idle we'll move the arm again.
        inTopHat = false;
    }
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The goal here is to respond to the buttons and joysticks on the game controller
// and adjust the motors and servos accordingly over and over forever.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
    InitializeRobot();

    while (true)   // forever
    {
        getJoystickSettings(joystick);

        HandleDriveJoysticks();
        HandleClawButtons();
        HandleArmTophat();

        wait1Msec(20);    // give the rest of system a chance to run
    }
}