1. StewartPlatform.cpp
#include <avr/io.h>
#include <Arduino.h>
#include <Servo.h>
#define SERVO0 3
#define SERVO1 5
#define SERVO2 6
#define SERVO3 9
#define SERVO4 10
#define SERVO5 11
#define HOME_PULSE 1500
#define SERVO_RATE 1600.0f/ PI
struct float3
{
float x,y,z;
};
struct float3x3
{
float a11,a12,a13,
a21,a22,a23,
a31,a32,a33;
};
void ServoPulse(Servo * pServos, int nServoNum, float fAngle, float fHomeAngle)
{
int servoTrims[6];
servoTrims[0] = 0;
servoTrims[1] = 0;
servoTrims[2] = 0;
servoTrims[3] = 0;
servoTrims[4] = 0;
servoTrims[5] = 0;
float angle = fAngle - radians(degrees(fHomeAngle) + servoTrims[nServoNum]);
if(angle > PI/6)
angle = PI/6;
else if(angle < -PI/6)
angle = -PI/6;
if(nServoNum % 2 == 0)
pServos[nServoNum].writeMicroseconds(HOME_PULSE - angle * SERVO_RATE);
else
pServos[nServoNum].writeMicroseconds(HOME_PULSE + angle * SERVO_RATE);
}
void RotationalMatrix(float3x3 * pRot, float fYaw, float fPitch, float fRoll)
{
float yaw = radians(fYaw);
float pitch = radians(fPitch);
float roll = radians(fRoll);

2. pRot->a11 = cos(yaw) * cos(roll);
pRot->a21 = sin(yaw) * cos(roll);
pRot->a31 = -sin(roll);
pRot->a12 = -sin(yaw) * cos(pitch) + cos(yaw) * sin(roll) * sin(pitch);
pRot->a22 = cos(yaw) * cos(pitch) + sin(yaw) * sin(roll) * sin(pitch);
pRot->a32 = cos(roll) * sin(pitch);
pRot->a13 = sin(yaw) * sin(pitch) + cos(yaw) * sin(roll) * cos(pitch);
pRot->a23 = -cos(yaw) * sin(pitch) + sin(yaw) * sin(roll) * cos(pitch);
pRot->a33 = cos(roll) * cos(pitch);
}
float LegLength(int nServoNum,float fHomeHeight, float3 * fTranslation, float3x3 *
fRotation, float3 * fBasePos, float3 * fPlatPos)
{
float3 rotXplat;
rotXplat.x = fRotation->a11 * fPlatPos[nServoNum].x + fRotation->a12 *
fPlatPos[nServoNum].y + fRotation->a13 * fPlatPos[nServoNum].z;
rotXplat.y = fRotation->a21 * fPlatPos[nServoNum].x + fRotation->a22 *
fPlatPos[nServoNum].y + fRotation->a23 * fPlatPos[nServoNum].z;
rotXplat.z = fRotation->a31 * fPlatPos[nServoNum].x + fRotation->a32 *
fPlatPos[nServoNum].y + fRotation->a33 * fPlatPos[nServoNum].z;
float3 leg;
leg.x = -fTranslation->x + rotXplat.x - fBasePos[nServoNum].x;
leg.y = -fTranslation->y + rotXplat.y - fBasePos[nServoNum].y;
leg.z = fHomeHeight - fTranslation->z + rotXplat.z - fBasePos[nServoNum].z;
return sqrt(leg.x * leg.x + leg.y * leg.y + leg.z * leg.z);
}
float ServoAngle(int nServoNum,float fHomeHeight, float fRod,float fHorn,float
fLeg,float3 * fBasePos,float3 * fPlatPos, int * nXAxisAngles)
{
float L = fLeg * fLeg - (fRod * fRod - fHorn * fHorn);
float M = 2 * fHorn * (fHomeHeight + fPlatPos[nServoNum].z);
float N = 2 * fHorn * (cos(radians(nXAxisAngles[nServoNum])) *
(fPlatPos[nServoNum].x - fBasePos[nServoNum].x) + sin(radians(nXAxisAngles[nServoNum])) *
(fPlatPos[nServoNum].y - fBasePos[nServoNum].y));
float angle = asin(L / sqrt(M * M + N * N)) - atan(N/M);
return angle;
}
int main(void)
{
init();
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
delay(500);
digitalWrite(13, LOW);
Serial.begin(57600);

3. Servo myServos[6];
float3 BasePositions[6];
{
BasePositions[0].x = -18.95f;
BasePositions[0].y = -54.32f;
BasePositions[0].z = 0;
BasePositions[1].x = 18.95f;
BasePositions[1].y = -54.32f;
BasePositions[1].z = 0;
BasePositions[2].x = 56.51f;
BasePositions[2].y = 10.75f;
BasePositions[2].z = 0;
BasePositions[3].x = 37.56f;
BasePositions[3].y = 43.57f;
BasePositions[3].z = 0;
BasePositions[4].x = -37.56f;
BasePositions[4].y = 43.57f;
BasePositions[4].z = 0;
BasePositions[5].x = -56.51;
BasePositions[5].y = 10.75f;
BasePositions[5].z = 0;
}
float3 PlatformPositions[6];
{
PlatformPositions[0].x = -29.39f;
PlatformPositions[0].y = -25.75f;
PlatformPositions[0].z = 0;
PlatformPositions[1].x = 29.39f;
PlatformPositions[1].y = -25.75f;
PlatformPositions[1].z = 0;
PlatformPositions[2].x = 37.0f;
PlatformPositions[2].y = -12.58f;
PlatformPositions[2].z = 0;
PlatformPositions[3].x = 7.61f;
PlatformPositions[3].y = 38.33f;
PlatformPositions[3].z = 0;
PlatformPositions[4].x = -7.61f;
PlatformPositions[4].y = 38.33f;
PlatformPositions[4].z = 0;
PlatformPositions[5].x = -37.0f;
PlatformPositions[5].y = -12.58f;
PlatformPositions[5].z = 0;
}
int XAxisAngles[6];
{

4. XAxisAngles[0] = 0;
XAxisAngles[1] = 180;
XAxisAngles[2] = 120;
XAxisAngles[3] = 300;
XAxisAngles[4] = 240;
XAxisAngles[5] = 60;
}
float rodLength = 72.5f;
float hornLength = 12.1f;
myServos[0].attach(SERVO0);
myServos[1].attach(SERVO1);
myServos[2].attach(SERVO2);
myServos[3].attach(SERVO3);
myServos[4].attach(SERVO4);
myServos[5].attach(SERVO5);
float homeHeight = sqrt(rodLength * rodLength + hornLength * hornLength -
(PlatformPositions[0].x - BasePositions[0].x) * (PlatformPositions[0].x -
BasePositions[0].x)
- (PlatformPositions[0].y - BasePositions[0].y) *
(PlatformPositions[0].y - BasePositions[0].y)) + PlatformPositions[0].z;
float3 Translation;
Translation.x = 0;
Translation.y = 0;
Translation.z = 0;
float Yaw = 0;
float Pitch = 0;
float Roll = 0;
float3x3 homeRot;
RotationalMatrix(&homeRot,Yaw,Pitch,Roll);
float homeAngle =
ServoAngle(0,homeHeight,rodLength,hornLength,LegLength(0,homeHeight,&Translation,&homeRot
,BasePositions,PlatformPositions),BasePositions,PlatformPositions,XAxisAngles);
for(int i = 0;i<6;i++)
ServoPulse(myServos,i,0,homeAngle);
while(1)
{
byte bData[26];
for (int i = 0; i < 26; i++)
bData[i] = 0;
if(Serial.available())
Serial.readBytes(bData,26);
if(bData[24] == 1 && bData[25] == 1)
{

5. memcpy(&Translation.x,&bData[0 * sizeof(float)],4);
memcpy(&Translation.y,&bData[1 * sizeof(float)],4);
memcpy(&Translation.z,&bData[2 * sizeof(float)],4);
memcpy(&Yaw,&bData[3 * sizeof(float)],4);
memcpy(&Pitch,&bData[4 * sizeof(float)],4);
memcpy(&Roll,&bData[5 * sizeof(float)],4);
}
float3x3 rotMatrix;
RotationalMatrix(&rotMatrix,Yaw,Pitch,Roll);
for(int i = 0;i<6;i++)
{
float leg =
LegLength(i,homeHeight,&Translation,&rotMatrix,BasePositions,PlatformPositions);
float angle =
ServoAngle(i,homeHeight,rodLength,hornLength,leg,BasePositions,PlatformPositions,XAxisAng
les);
if(!isnan(angle))
ServoPulse(myServos,i,angle,homeAngle);
}
if (serialEventRun) serialEventRun();
}
}