#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
using namespace std;
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105
#define pi 3.14159265358979
#define radians(deg) ((deg)*DEG_TO_RAD)
#define degrees(rad) ((rad)*RAD_TO_DEG)
//static float theta_r = 10;
// static float theta_s[6]={150,-90,30, 150,-90,30};
// static float theta_p = 30;
// static float RD = 15.75;
// static float PD = 16;
// static float ServoArmLengthL1 = 7.25;
// static float ConnectingArmLengthL2 = 28.5;
// static float platformHeight = 25.5170749;
static float theta_r = 51.25;
static float theta_s[6]={0,60,-120,-60,120,180};
static float theta_p = 37.845;
static float RD = 65.42;
static float PD = 69.642;
static float ServoArmLengthL1 = 21;
static float ConnectingArmLengthL2 = 59;
static float platformHeight = 46;
static float servoPulseMultiplierPerRadian = 800/(pi/4);
//static float arr[6]={0,0,-4.86, 0,0,0};
static float arr[6]={0,0,0, 0,0,0};
//need to constrain values to limits, ensure ballance, same degree/-degree at top low limits, ...
//helpers for Platform Coord algorithm
float platformPDx[6]={0,0,0, 0,0,0};
float platformPDy[6]={0,0,0, 0,0,0};
float platformAngle[6]={0,0,0,0,0,0};
float platformCoordsx[6]={0,0,0, 0,0,0};
float platformCoordsy[6]={0,0,0, 0,0,0};
float basePDx[6]={0,0,0, 0,0,0};
float basePDy[6]={0,0,0, 0,0,0};
float baseAngle[6]={0,0,0,0,0,0};
float baseCoordsx[6]={0,0,0, 0,0,0};
float baseCoordsy[6]={0,0,0, 0,0,0};
float DxMultiplier[6]={1,1,1, -1,-1,-1};
float AngleMultiplier[6]={1,-1,1,1,-1,1};
float OffsetAngle[6]={pi/6,pi/6,-pi/2, -pi/2,pi/6,pi/6};
//base rotation values
float platformPivotx[6]={0,0,0, 0,0,0};
float platformPivoty[6]={0,0,0, 0,0,0};
float platformPivotz[6]={0,0,0, 0,0,0};
float deltaLx[6] = {0,0,0, 0,0,0};
float deltaLy[6] = {0,0,0, 0,0,0};
float deltaLz[6] = {0,0,0, 0,0,0};
float deltaL2Virtual[6] = {0,0,0, 0,0,0};
float l[6] = {0,0,0, 0,0,0};
float m[6] = {0,0,0, 0,0,0};
float n[6] = {0,0,0, 0,0,0};
float alpha[6] = {0,0,0, 0,0,0};
float mapfloat(double x, double in_min, double in_max, double out_min, double out_max)
{
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}
unsigned char setPos(float pe[]){
unsigned char errorcount;
errorcount=0;
printf("Alpha\n");
//Platform and Base Coords
for(int i = 0; i < 6; i++)
{
platformPDx[i] = DxMultiplier[i] * RD;
platformPDy[i] = RD;
platformAngle[i] = OffsetAngle[i] + AngleMultiplier[i]* radians(theta_r);
platformCoordsx[i] = platformPDx[i] * cos(platformAngle[i]);
platformCoordsy[i] = platformPDy[i] * sin(platformAngle[i]);
basePDx[i] = DxMultiplier[i] * PD;
basePDy[i] = PD;
baseAngle[i] = OffsetAngle[i] + AngleMultiplier[i]* radians(theta_p);
baseCoordsx[i] = basePDx[i] * cos(baseAngle[i]);
baseCoordsy[i] = basePDy[i] * sin(baseAngle[i]);
//Platform pivots
platformPivotx[i] = platformCoordsx[i]*cos(arr[3])*cos(arr[5])+platformCoordsy[i]*(sin(arr[4])*sin(arr[3])*cos(arr[3])-cos(arr[4])*sin(arr[5]))+arr[0];
platformPivoty[i] = platformCoordsx[i]*cos(arr[3])*sin(arr[5])+platformCoordsy[i]*(cos(arr[4])*cos(arr[5])+sin(arr[4])*sin(arr[3])*sin(arr[5]))+arr[1];
platformPivotz[i] = -platformCoordsx[i]*sin(arr[3])+platformCoordsy[i]*sin(arr[4])*cos(arr[3])+platformHeight+arr[2];
deltaLx[i] = baseCoordsx[i] - platformPivotx[i];
deltaLy[i] = baseCoordsy[i] - platformPivoty[i];
deltaLz[i] = -platformPivotz[i];
deltaL2Virtual[i] = sqrt(pow(deltaLx[i], 2.0) + pow(deltaLy[i], 2.0) +pow(deltaLz[i], 2.0));
l[i] = pow(deltaL2Virtual[i], 2.0)-(pow(ConnectingArmLengthL2, 2.0)-pow(ServoArmLengthL1, 2.0));
m[i] = 2*ServoArmLengthL1*(platformPivotz[i]);
n[i] = 2*ServoArmLengthL1*(cos(theta_s[i]*pi/180)*(platformPivotx[i] - baseCoordsx[i])+sin(theta_s[i]*pi/180)*(platformPivoty[i]- baseCoordsy[i]));
alpha[i]=asin(l[i]/(sqrt(pow(m[i], 2.0)+pow(n[i], 2.0))))-atan(n[i]/m[i]);
printf("%.10f", alpha[i]);
printf(",");
}
return errorcount;
}
void process_data ( char * data)
{
int i = 0;
char *tok = strtok(data, ",");
printf("Input\n");
while (tok != NULL) {
double value = (double)atof(tok);
double temp = 0.0;
if(i == 2)
temp =mapfloat(value, 0, 4094, -7, 7);//hieve
else if(i > 2)//rotations, pitch,roll,yaw
temp = mapfloat(value, 0, 4094, -30, 30) *((double)pi/(double)180.0);
else//sway,surge
temp = mapfloat(value, 0, 4094, -8, 8);
arr[i++] = (float)temp;
printf("%.11f", temp);
printf(",");
tok = strtok(NULL, ",");
}
printf("\n");
setPos(arr);
}
int main()
{
// char value[] = "2047,2047,2047,2047,2047,2047";
char value[] = "2047,2047,2047,2047,2047,2047";
process_data (value);
return 0;
}
