# PS2 Controlled Holonomic Platform Using RobotC

GD Star Rating

### 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 Omni-wheels 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.

• 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® Omni-wheels. Those robots where coded with the NXC Programming Language. After a few requests for help with RobotC Programming Language code for Holonomic platforms, I decided to through some code together, which I hope will help people new to the RobotC Programming Language & wanting to build their own Holonomic platforms.

The Holonomic platform is controlled with a Mindsensors® 'Sony PlayStation 2 Controller interface for NXT (PSP-Nx-v4)', 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:

```#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 &#39;ROBOTC&#39; configuration wizard               !!*//

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

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

#include  &quot;PSP-Nx-lib.c&quot;

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

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

{
eraseDisplay();
psp currState;
SensorType[PS2] = sensorI2CCustom9V;

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

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

V1 = Vx;                                         // Vector Calculation for MotorA(V1)&#39;s Power
if (V1 &lt; 20 &amp;&amp; V1 &gt; 0) {V1 = 0;}                 // Set Minimum MotorA&#39;s Forward Power
if (V1 &lt; 0 &amp;&amp; V1 &gt; -20) {V1 = 0;}                // Set Minimum MotorA&#39;s Reverse Power
if (V1 &gt; 100 || V1 &lt; -100) {V1 = 100;}           // Set Maximum Motor Power Level at 100

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

V3 = -Vx / 2 + sqrt(3)/2 * Vy;                   // Vector Calculation for MotorC(V3)&#39;s Power
if (V3 &lt; 20 &amp;&amp; V3 &gt; 0) {V3 = 0;}                 // Set Minimum MotorC&#39;s Forward Power
if (V3 &lt; 0 &amp;&amp; V3 &gt; -20) {V3 = 0;}                // Set Minimum MotorC&#39;s Reverse Power
if (V3 &gt; 100 || V3 &lt; -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, &quot;MotorA: %d&quot;, PowerA);     // Display MotorA&#39;s Power Setting
nxtDisplayTextLine(6, &quot;MotorB: %d&quot;, PowerB);     // Display MotorB&#39;s Power Setting
nxtDisplayTextLine(7, &quot;MotorC: %d&quot;, PowerC);     // Display MotorC&#39;s Power Setting

motor[motorA] = PowerA;                          // Set MotorA&#39;s Velocity (Power Setting)
motor[motorB] = PowerB;                          // Set MotorB&#39;s Velocity (Power Setting)
motor[motorC] = PowerC;                          // Set MotorC&#39;s Velocity (Power Setting)
}
}
```

GD Star Rating