Arduiono 6DOF simulator

I am a newbe here doing my final project. I want to use arduino mega for the positioning and the inverse kinematics and hoping to use SImtools for extraction and conversion. X-Sim is so hard to understand. ANybody with the way to go for setups?

here is my adopted arduino code


#include <Servo.h>


//MIN and MAX PWM pulse sizes of Servos(Parallax 900-00005)
#define MAX 2250
#define MIN 750

//Positions of servos mounted in opposite direction
#define INV1 0
#define INV2 2
#define INV3 4

//constants for computation of positions of connection points
#define pi 3.14159
#define deg2rad 180/pi
#define deg30 pi/6

//Array of servo objects
Servo servo[6];

//Zero positions of servos, in this positions their arms are perfectly horizontal, in us
static int zero[6]={1500,1500,1500,1500,1500,1500};

//In this array is stored requested position for platform - x,y,z,rot(x),rot(y),rot(z)
static float arr[6]={0,0,0, radians(0),radians(0),radians(0)};



//Actual degree of rotation of all servo arms, they start at 0 - horizontal, used to reduce
//complexity of calculating new degree of rotation
static float theta_a[6]={0.0,0.0,0.0, 0.0,0.0,0.0};

//Array of current servo positions in us
static int servo_pos[6];

//rotation of servo arms in respect to axis x
const float beta[] = {0, pi, -(2 * pi/3), (pi/3), (2 * pi/3), -(pi/3)},

//maximum servo positions, 0 is horizontal position
servo_min=radians(-70),servo_max=radians(70),

//servo_mult - multiplier used for conversion radians->servo pulse in us
//L1-effective length of servo arm, L2 - length of base and platform connecting arm
//z_home - height of platform above base, 0 is height of servo arms
servo_mult=400/(pi/4), L1 = 1.48, L2 = 8.67, z_home = 7.48;

//RD distance from center of platform to attachment points (arm attachment point)
//RD distance from center of base to center of servo rotation points (servo axis)
//theta_p-angle between two servo axis points,
//theta_r - between platform attachment points
//theta_angle-helper variable
//p[][]=x y values for servo rotation points
//re[]{}=x y z values of platform attachment points positions
//equations used for p and re will affect postion of X axis, they can be changed to achieve
//specific X axis position
const float RD = 5.51,PD = 5.51,theta_p = radians(70),
theta_angle=radians(6), theta_r = radians(25),
p[2][6]={
{
-3.15,3.15,
5.12,2.17,
-2.17,-5.12
},
{
4.33,4.33,
0.59,-4.33,
-4.33,0.59
}
},
re[3][6] = {
{
-3.94,3.94,
5.12,1.18,
-1.18,-5.12,
},{
3.54, 3.54,
1.38,-5.31,
-5.31,1.38,
},{
0,0,0,0,0,0
}
};
//arrays used for servo rotation calculation
//H[]-center position of platform can be moved with respect to base, this is
//translation vector representing this move
static float M[3][3], rxp[3][6], T[3], H[3] = {0,0,z_home};


void setup(){

//attachment of servos to PWM digital pins of arduino
servo[0].attach(9, MIN, MAX);
servo[1].attach(3, MIN, MAX);
servo[2].attach(5, MIN, MAX);
servo[3].attach(11, MIN, MAX);
servo[4].attach(6, MIN, MAX);
servo[5].attach(10, MIN, MAX);

//begin of serial communication
Serial.begin(9600);

//reminder to include lines for centering platform
//putting into base position
setPos(arr);
delay(3000);

}

//function calculating needed servo rotation value
float getAlpha(int *i){
static int n;
static float th=0;
static float q[3], dl[3], dl2;
double min=servo_min;
double max=servo_max;
n=0;
th=theta_a[*i];
while(n<20){
//calculation of position of base attachment point (point on servo arm where is leg connected)
q[0] = L1*cos(th)*cos(beta[*i]) + p[0][*i];
q[1] = L1*cos(th)*sin(beta[*i]) + p[1][*i];
q[2] = L1*sin(th);

//calculation of distance between according platform attachment point and base attachment point
dl[0] = rxp[0][*i] - q[0];
dl[1] = rxp[1][*i] - q[1];
dl[2] = rxp[2][*i] - q[2];
dl2 = sqrt(dl[0]*dl[0] + dl[1]*dl[1] + dl[2]*dl[2]);

//if this distance is the same as leg length, value of theta_a is correct, we return it
if(abs(L2-dl2)<0.01){
return th;
}
//if not, we split the searched space in half, then try next value
if(dl2<L2){
max=th;
}else{
min=th;
}
n+=1;
if(max==servo_min || min==servo_max){
return th;
}
th = min+(max-min)/2;
}
return th;
}

//function calculating rotation matrix
void getmatrix(float pe[])
{
float psi=pe[5];
float theta=pe[4];
float phi=pe[3];
M[0][0] = cos(psi)*cos(theta);
M[1][0] = -sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi);
M[2][0] = sin(psi)*sin(phi)+cos(psi)*cos(phi)*sin(theta);

M[0][1] = sin(psi)*cos(theta);
M[1][1] = cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi);
M[2][1] = -cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);

M[0][2] = -sin(theta);
M[1][2] = cos(theta)*sin(phi);
M[2][2] = cos(theta)*cos(phi);
}

//calculates wanted position of platform attachment points using calculated rotation matrix
//and translation vector
void getrxp(float pe[])
{
for(int i=0;i<6;i++){
rxp[0] = T[0]+M[0][0]*(re[0])+M[0][1]*(re[1])+M[0][2]*(re[2]);
rxp[1] = T[1]+M[1][0]*(re[0])+M[1][1]*(re[1])+M[1][2]*(re[2]);
rxp[2] = T[2]+M[2][0]*(re[0])+M[2][1]*(re[1])+M[2][2]*(re[2]);
}
}
//function calculating translation vector - desired move vector + home translation vector
void getT(float pe[])
{
T[0] = pe[0]+H[0];
T[1] = pe[1]+H[1];
T[2] = pe[2]+H[2];
}

unsigned char setPos(float pe[]){
unsigned char errorcount;
errorcount=0;
for(int i = 0; i < 6; i++)
{
getT(pe);
getmatrix(pe);
getrxp(pe);
theta_a=getAlpha(&i);
if(i==INV1||i==INV2||i==INV3){
servo_pos = constrain(zero - (theta_a)*servo_mult, MIN,MAX);
}
else{
servo_pos = constrain(zero + (theta_a)*servo_mult, MIN,MAX);
}
}

for(int i = 0; i < 6; i++)
{
if(theta_a==servo_min||theta_a[i]==servo_max||servo_pos[i]==MIN||servo_pos[i]==MAX){
errorcount++;
}
servo[i].writeMicroseconds(servo_pos[i]);
}
return errorcount;
}


//main control loop, obtain requested action from serial connection, then execute it
void loop()
{

if(Serial.available()>0){
int input=Serial.read();

//action to change position of platform, obtain 6 values representing desired position
for(int i=0;i<6;i++){
long kk;
while(Serial.available()<4){
;
}
kk=(long)Serial.read();
kk=kk+(Serial.read()<<8);
kk=kk+(Serial.read()<<16);
kk=kk+(Serial.read()<<24);
if(i<3){
arr[i]=(kk/100)/25.4;
}else{
arr[i]=radians(kk/100.0);
}
}
Serial.write(setPos(arr));
Serial.flush();

//return current position of platform
retPos();
}
}

void retPos(){
for(int i=0;i<6;i++){
long val;
if(i<3){
val=(long)(arr[i]*100*25.4);
}else{
val=(long)(arr[i]*100*deg2rad);
}
Serial.write(val);
Serial.write((val>>8));
Serial.write((val>>16));
Serial.write((val>>24));
Serial.flush();
}
}[/i][/i][/i][/i][/i][/i][/i][/i][/i]

I am now using Arduino UNO and here are my setups
any insights please?

Use the smc3 code

I use 3 nanos each one controls 2 actuators

It works great !

I have built a single board that can control 6 motors, not found time to test it

My budget is tight and I have now week to finalize. I have one Uno. I have done the inverse kinematics as shown in the code. i Just need corrections on the code, and the simtools setup. or if anybody has used x-sim, the better

okay let me send my interface setup and the axis setup as well. Please assist me get on the right track. Note, the output testing also not working. What might be the problem?








getting data to the engine too

The Uno does not have enough pins to run a 6dof sim

Does your code use pid control ?

How does it compensate for the cog of the driver ?

I think you can use simtools to convert rotary motion to linear motion

what do you mean? I have enough PWM pins

//attachment of servos to PWM digital pins of arduino
servo[0].attach(9, MIN, MAX);
servo[1].attach(3, MIN, MAX);
servo[2].attach(5, MIN, MAX);
servo[3].attach(11, MIN, MAX);
servo[4].attach(6, MIN, MAX);
servo[5].attach(10, MIN, MAX);

let me try it out. are the axis okay?

I intend to incorporate PID. Its in the pipeline. for now I just need to have some movements.

you will need analogue feedback pin, pwm pin and direction pin for each motor

https://www.xsimulator.net/community/threads/6dof-tiny-stewart-platform-rc-servos-arduino-uno.12287/