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);

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);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];{

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){

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,XAxisAngles);

if(!isnan(angle))ServoPulse(myServos,i,angle,homeAngle);

}

if (serialEventRun) serialEventRun(); }}