PS2 Controlled Holonomic Platform Using RobotC
Yes, another Holonomic Platform. My son, Anthony requested a Holonomic robot to take to school for showandtell. So here it is with a few scholarly extras, including the [robotc] code.
A Holonomic platform is one of the many types of Holonomic drive trains — it can move forward and backward as well as left or right without turning. The Holonomic platform can move in any direction instantaneously unlike conventional four wheeled vehicles.
The Holonomic platform is based on 3 wheels offset 120 degrees from each other, using omni wheels (wheels with horizontal rollers on the side so they can slide left or right but be powered forward).
Holonomic refers to the relationship between controllable, and total degrees of freedom of a robot. If the controllable degree of freedom is equal to total degrees of freedom, then the robot is said to be Holonomic. A robot built on castor wheels or Omniwheels is a good example of Holonomic drive as it can freely move in any direction and the controllable degrees of freedom is equal to total degrees of freedom.
A three wheel Holonomic design offers great traction, as any reactive force is distributed through only three points and the robot is well balanced even on uneven terrain. This design also reduces an additional wheel compared to a 4 wheeled robot which makes it cost effective (yes, these wheels are expensive). A three wheeled Holonomic robot can drive more straight than a conventional four wheeled robot.
Now for Some Math:
In a rigid body translation without rotation means that every point on the body has the same velocity as the center of mass of the whole body. Thus, for each motor we have two equations relating the velocity (e.g. V1) and the perpendicular velocity (e.g. V1′) with the projections of Omni velocity in the X and Y directions have – V sin(θ) and V cos(θ), respectively. In the following we denote these two velocity components simply as Vx and Vy.
For the first motor (the one in the forward direction) we have:

Vx=V1

Vy=–V1
For the second wheel:

Vx=–V2 cos(60°) – V2′ cos(30°)

Vy=–V2 sin(60°) + V2′ sin(30°)
And finally for the third wheel:

Vx=–V3 sin(30°) + V3′ cos(30°)

Vy=V3 cos(30°) + V3′ sin(30°)
These set of 6 unknowns (V1, V2, V3 and the three primed velocities) and 6 equations has exactly one solution:

V1=Vx

V2=–Vx / 2 – Sqrt(3)/2 Vy

V3=–Vx / 2 + Sqrt(3)/2 Vy {Where Sqrt(3) is just the squared root of 3.}
In previous posts I have shown Holonomic platforms based on 3 wheels offset 120 degrees from each other, using [rotacaster]. Those robots where coded with the [nxc]. After a few requests for help with [robotc] code for Holonomic platforms, I decided to through some code together, which I hope will help people new to the [robotc] & wanting to build their own Holonomic platforms.
The Holonomic platform is controlled with a [mindsensors] ‘Sony PlayStation 2 Controller interface for NXT (PSPNxv4)‘, and a PS2 Game Controller. Also the Robot can be controlled from the RobotC IDE (programming environment) via BlueTooth, using a generic PC type ‘Game Controller’.
The following RobotC Code shows an example of how to code for Killough & Holonomic Platforms:
Download Source Code
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 
#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 "PSPNxlib.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" Coordinates Vy = (int)currState.l_j_y; // Read the PS2 Right Joystick's "Y" Coordinates 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) } } 
'Melon Factor'  Conclusion
This was to be Part 4/5 of my Melon Factor Project, for the NXTLog FoodFactor Competition for May 2012. It's unfortunate, but time has not been kind. I had more ... Read more
RobotC: Using Multiple Ultrasonic (Sonar) Sensors
When using Multiple Ultrasonic (Sonar) Sensors together, you need to configure the Sensors to use 'Single Shot Mode', so only ONE Sensor is taking a measurement at any time. If ... Read more
Leave a Reply