Hey there! So I am working on a project to build a 6-DOF Motion Platform. I went for the Stewart-Platform "method". I've build it and have all the parts together as far as possible in the testing phase. I am using linear actuators https://shop.boxdrive.ch/p/DLA-24-5-A-300-POT-IP65.aspx Datasheets and more can be found on their website. I am using an Arduino Mega 2560 with 6 IBT2. The calculation of coordinates works and is correct, and the actuators can be reliably controlled and they work fine. However, I am stuck at the speed calculation. Obviously I want all the actuators to reach their target positions simultaneously, so the platform does not break, because of invalid positions during the "driving-phase". I would really appreciate it, if you guys could give me some tips! Thanks in advance! My code:
#include <math.h>
float RbMatrix[3][6];
float sMatrix[3][6];
float maxlist[6];
//float ActuatorBaseLength = 450;
float bMatrix[3][6] = {
{-70,-330,-285,285,330,70},
{-350,125,230,230,125,-350},
{-88,-88,-88,-88,-88,-88}
};
float aMatrix[3][6] = {
{-285,-330,-70,70,330,285},
{-230,-125,350,350,-125,-230},
{88,88,88,88,88,88}
};
float newspeed[6];
float speed[6];
float s[6];
float news[6];
float mf[6];
float turnmode[6];
int rpwm[6] = { 2,13,4,7,9,11 };
int lpwm[6] = { 3,12,5,6,8,10 };
int m[6] = { A5,A4,A3,A2,A1,A0 };
int abweichungunten[6] = { 25,18,25,21,21,23 };
int abweichungoben[6] = { 938,940,946,937,941,938 };
float current;
int maxI;
float speedmax;
float max = 0;
float longestway;
float actfullin = 0;
float actfullout = 305;
float adeg = 0; //z
float bdeg = 0; //y
float cdeg = 0; //x
float a;
float b;
float c;
float RMatrix[3][3];
float time[6];
float pMatrix[3][1] = {
{0},
{0},
{600} //11.3 sek für rein mit voll geschwindigkeit
};
void setup()
{
for (int i = 0; i < 6; i++) {
pinMode(rpwm[i], OUTPUT);
pinMode(lpwm[i], OUTPUT);
}
Serial.begin(9600);
}
// Add the main program code into the continuous loop() function
void loop()
{
RMatrixCalc();
ReadFeedback();
LengthCalc();
anpassung();
RequestedPosMapping();
SpeedCalc();
Execution();
Serial.print("Feedbacks: ");
for (int i = 0; i < 6; i++) {
Serial.print(mf[i]);
Serial.print(" , ");
}
Serial.println();
//Serial.print(s1 - 450);
//Serial.print(" , m1f:");
//Serial.print(m1f);
//Serial.print(" , ");
//Serial.println(news1);
}
void RMatrixCalc() {
a = adeg * 1000 / 57296;
b = bdeg * 1000 / 57296;
c = cdeg * 1000 / 57296;
RMatrix[0][0] = (cos(a) * cos(b));
RMatrix[0][1] = (cos(a) * cos(b) * sin(c) - sin(a) * cos(c));
RMatrix[0][2] = (cos(a) * sin(b) * cos(c) + sin(a) * sin(c));
RMatrix[1][0] = (sin(a) * cos(b));
RMatrix[1][1] = (sin(a) * sin(b) * sin(c) + cos(a) * cos(c));
RMatrix[1][2] = (sin(a) * sin(b) * cos(c) - cos(a) * sin(c));
RMatrix[2][0] = (-sin(b));
RMatrix[2][1] = (cos(b) * sin(c));
RMatrix[2][2] = (cos(b) * cos(c));
}
void LengthCalc() {
for (int j = 0; j < 6; j++) { //amount of motors (1 column is 1 motor)
for (int i = 0; i < 3; i++)//rows
{
RbMatrix[i][j] = (RMatrix[i][0] * bMatrix[0][j]) + (RMatrix[i][1] * bMatrix[1][j]) + (RMatrix[i][2] * bMatrix[2][j]);
}
//p+Rbi-ai
for (int i = 0; i < 3; i++) //rows, columns not need because always 1, resp. 0
{
sMatrix[i][j] = pMatrix[i][0] + RbMatrix[i][j] - aMatrix[i][j];
}
//Calculation of absolute value of vector
s[j] = sqrt(pow(sMatrix[0][j], 2) + pow(sMatrix[1][j], 2) + pow(sMatrix[2][j], 2));
}
}
void RequestedPosMapping() {
//map the required actuator lengths whcih are needed in order to reach the requested position onto a compatible scale to compare with current locations
for (int i = 0; i < 6; i++) {
news[i] = map(s[i], 450 + actfullin, 450 + actfullout, abweichungunten[i], abweichungoben[i]); //25 and 938 are the real limits of motors when they stop/hit the stop switches
}
}
void ReadFeedback() {
for (int i = 0; i < 6; i++) {
mf[i] = analogRead(m[i]);
}
}
void anpassung() {
s[0] = s[0] - 2;
//s[1] = s[1] + ;
s[2] = s[2] - 5;
s[3] = s[3] - 3;
s[4] = s[4] - 2;
//s[5] = s[5] + ;
}
void Execution() {
for (int i = 0; i < 6; i++) {
if (news[i] - mf[i] > 0)
{
//if requested position is further back than actual position, set turnmode to backwards
turnmode[i] = 0;
}
else if ((news[i] - mf[i] < 30) && (news[i] - mf[i] > -30)) {
turnmode[i] = 2;
}
else
{
//if requested position is further out than actual position, set turnmode to forwards
turnmode[i] = 1;
}
if (turnmode[i] == 1) {
analogWrite(rpwm[i], 0);
analogWrite(lpwm[i], newspeed[i]);
}
else if (turnmode[i] == 2) {
analogWrite(rpwm[i], 0);
analogWrite(lpwm[i], 0);
}
else {
analogWrite(rpwm[i], newspeed[i]);
analogWrite(lpwm[i], 0);
}
}
}
void SpeedCalc() {
//find out which one is the biggest distance to drive
max = 0;
for (int i = 0; i < 6; i++) {
maxlist[i] = abs(news[i] - mf[i]); //maxlist = absoluti distanz
}
for (int i = 0; i < 6; i++) {
if (maxlist[i] > max) {
max = maxlist[i];
maxI = i;
}
}
Serial.print(" MaxI: ");
Serial.print(maxI);
Serial.print(" Speeds: ");
for (int i = 0; i < 6; i++) {
newspeed[i] = map(maxlist[i], 0, maxlist[maxI], 55, 225);
Serial.print(newspeed[i]);
Serial.print(" , ");
}
Serial.println();
}
