PS2 Controlled Holonomic Platform Using RobotC

#pragma config(Sensor, S4,     PS2,     sensorI2CCustom9V)
#pragma config(Motor,  motorA,          tmotorNormal, openLoop)
#pragma config(Motor,  motorB,          tmotorNormal, openLoop)
#pragma config(Motor,  motorC,          tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


/**********************************************************
*
*  Omni velocity in the X and Y directions:
*  V sin(?) and  V cos(?), respectively....
*
*         V1=Vx
*         V2=–Vx / 2 – Sqrt(3)/2 Vy
*         V3=–Vx / 2 + Sqrt(3)/2 Vy
*
*
*              OUT_A
*                V1
*
*            V3     V2
*          OUT_B   OUT_C
*
***********************************************************


***********************************************************
*              Set Constants and Variables                *
***********************************************************/

#define   Addr 0x02
#include  "PSP-Nx-lib.c"

float     V1, V2, V3, Vx, Vy;
int       PowerA, PowerB, PowerC;


/**********************************************************
*              Main Killough Platform Control             *
***********************************************************/

task main()
{
    eraseDisplay();
    psp currState;
    nI2CBytesReady[PS2] = 0;
    SensorType[PS2] = sensorI2CCustom9V;

    while (true)
    {
       nxtDisplayCenteredBigTextLine(0, "Sony PS2");
       nxtDisplayCenteredBigTextLine(2, "Killough");

       PSP_ReadButtonState(PS2, Addr, currState);       // Read in the PS2 Controller Data

       Vx = (int)currState.l_j_x;                       // Read the PS2 Left Joystick's "X" Co-ordinates
       Vy = (int)currState.l_j_y;                       // Read the PS2 Right Joystick's "Y" Co-ordinates

       V1 = Vx;                                         // Vector Calculation for MotorA(V1)'s Power
       if (V1 < 20 && V1 > 0) {V1 = 0;}                 // Set Minimum MotorA's Forward Power
       if (V1 < 0 && V1 > -20) {V1 = 0;}                // Set Minimum MotorA's Reverse Power
       if (V1 > 100 || V1 < -100) {V1 = 100;}           // Set Maximum Motor Power Level at 100

       V2 = -Vx / 2 - sqrt(3)/2 * Vy;                   // Vector Calculation for MotorB(V2)'s Power
       if (V2 < 20 && V2 > 0) {V2 = 0;}                 // Set Minimum MotorB's Forward Power
       if (V2 < 0 && V2 > -20) {V2 = 0;}                // Set Minimum MotorB's Reverse Power
       if (V2 > 100 || V2 < -100) {V2 = 100;}           // Set Maximum Motor Power Level at 100

       V3 = -Vx / 2 + sqrt(3)/2 * Vy;                   // Vector Calculation for MotorC(V3)'s Power
       if (V3 < 20 && V3 > 0) {V3 = 0;}                 // Set Minimum MotorC's Forward Power
       if (V3 < 0 && V3 > -20) {V3 = 0;}                // Set Minimum MotorC's Reverse Power
       if (V3 > 100 || V3 < -100) {V3 = 100;}           // Set Maximum Motor Power Level at 100

       PowerA = V1;       // Convert Floation Point Power Calculation to an Integer Value for MotorA
       PowerB = V2;       // Convert Floation Point Power Calculation to an Integer Value for MotorB
       PowerC = V3;       // Convert Floation Point Power Calculation to an Integer Value for MotorC

       nxtDisplayTextLine(5, "MotorA: %d", PowerA);     // Display MotorA's Power Setting
       nxtDisplayTextLine(6, "MotorB: %d", PowerB);     // Display MotorB's Power Setting
       nxtDisplayTextLine(7, "MotorC: %d", PowerC);     // Display MotorC's Power Setting
       
       motor[motorA] = PowerA;                          // Set MotorA's Velocity (Power Setting)
       motor[motorB] = PowerB;                          // Set MotorB's Velocity (Power Setting)
       motor[motorC] = PowerC;                          // Set MotorC's Velocity (Power Setting)
    }
}