Hardware components | ||||||
| × | 5 | ||||
 |
| × | 1 | |||
Software apps and online services | ||||||
 |
| |||||
What is that?
Mechanical Parts
Photos
Motion
Read moreCloudie is a five degrees of freedom robotic arm, I made during the summer break 2016. Because of the fact that it is closed kinematic chain it has some pros which are not occurring when the kinematic chain is open, such as e.g. larger possible load.
What is Cloudie Made Of?It is made of parts printed on the 3D printer, five servos, motor with encoder and many screws. Four out of 6 drives is located on the base of the entire construction. It allowed to reduce mass of the moving arm. Therefore the manipulator is able to move with higher precision and forces affecting Cloudie are lower.
AssemblingI strongly recommend to assemble the manipulator from base to the gripper otherwise it is possible that you will have some troubles with connecting moving base and arm. Generally it isn't challenging except one step. Assembling gripper. Picture below is showing how to do this.
Rest of the assembling is just to attach next parts from the base to gripper.
ControllingI wrote three codes for three ways to control Cloudie.
- The first one is to control every drive separately. - It works as in an excavator.
- The second one is a way to control Cloudie with coordinates. - There are four parameters to be set. X axis, Y axis, Z axis and angle between gripper and base.
- The third one make Cloudie to move on rectilinear path. - After holding one button Cloudie is moving in appropriate axis.
Parts required to the project are designed in the open software OpenSCAD program. Additional parts:
- 4 x servo TowerPro MG995
- 1 x servo TowerPro MG90S,
- 1 DC motor with encoder,
- 1 x toothed belt GT2,
- many M3 screws, nuts and spacer discs,
- many servo screws
I used ASG balls - diameter 6mm to make bearing.
Cloudie rectilinear control:
Cloudie coordinates movement:
Cloudie separate motion:
//Author: Piotr Paterek, Kraków 05-08.2016
#include "hFramework.h"
#include "hCloudClient.h"
bool keyW = false;
bool keyS = false;
bool keyA = false;
bool keyD = false;
hPIDRegulator pidReg;
int i = 0;
int m = 0;
int n = 0;
float s;
int mot1 = 0;
int mot2 = 0;
float angle1 = -5;
float angle2 = 10;
float angle3 = 13;
float angle4 = -5;
float angle5 = -11;
hGPIO& lightLeft = hSens1.pin1;
hGPIO& lightRight = hSens2.pin1;
void debugConsole_task()
{
char ch;
platform.printf("Wpisz znak:\r\n");
for (;;) {
if (platform.read(&ch, 1) == 1) {
platform.printf("echo: %c\r\n", ch);
}
}
}
void status_task()
{
while (1) {
platform.ui.label("lb_bat").setText("%2f V", sys.getSupplyVoltage());
platform.ui.progressBar("pb_bat").setValue(sys.getSupplyVoltageMV() / 15); //supply voltage milivolts
sys.delay(200);
}
}
void motorMotion()//pid regulator for the motor
{
// hPID
pidReg.setScale(1);
pidReg.setKP(39.0);
pidReg.setKI(0.05);
pidReg.setKD(1000);
// hRegulator
pidReg.dtMs = 5;
pidReg.stableRange = 10;
pidReg.stableTimes = 3;
hMot1.attachPositionRegulator(pidReg);
}
void cfgHandler()
{
auto l1 = platform.ui.label("l1");
auto l2 = platform.ui.label("l2");
auto b = platform.ui.button("btn1");
auto slider = platform.ui.input("slider1");
auto input = platform.ui.input("text");
platform.ui.loadHtml({Resource::WEBIDE, "/ui.html"});
// platform.ui.video.enable();
}
void servoMotionSeparately()
{
hServoModule.s1.calibrate(-90, 2400, 90, 800); // servos calibration
hServoModule.s2.calibrate(-90, 2400, 90, 800);
hServoModule.s3.calibrate(-90, 800, 90, 2400);
hServoModule.s4.calibrate(-90, 800, 90, 2400);
hServoModule.s5.calibrate(-90, 800, 90, 2400);
while (1) {
sys.delay(10);
if (i == 1) { //following angles on servos with limits
angle1 += 2;
if (angle1 > -5) {
angle1 = -5;
}
sys.delay(30);
platform.ui.console("cl1").printf("Angle3 = %f \n", angle3);
platform.ui.console("cl1").printf("Angle2 = %f \n", angle2);
platform.ui.console("cl1").printf("Angle1 = %f \n", angle1);
platform.ui.console("cl1").printf("--------------------------\n");
hServoModule.s1.rotAbs(angle1);
} else if (i == 2) {
angle1 -= 2;
if (angle1 < -90) {
angle1 = -90;
}
sys.delay(30);
platform.ui.console("cl1").printf("Angle3 = %f \n", angle3);
platform.ui.console("cl1").printf("Angle2 = %f \n", angle2);
platform.ui.console("cl1").printf("Angle1 = %f \n", angle1);
platform.ui.console("cl1").printf("--------------------------\n");
hServoModule.s1.rotAbs(angle1);
} else if (i == 3) {
angle2 += 2;
if (angle2 > 83) {
angle2 = 83;
}
sys.delay(30);
platform.ui.console("cl1").printf("Angle3 = %f \n", angle3);
platform.ui.console("cl1").printf("Angle2 = %f \n", angle2);
platform.ui.console("cl1").printf("Angle1 = %f \n", angle1);
platform.ui.console("cl1").printf("--------------------------\n");
hServoModule.s2.rotAbs(angle2);
} else if (i == 4) {
angle2 -= 2;
if (angle2 < -10) {
angle2 = -10;
}
sys.delay(30);
platform.ui.console("cl1").printf("Angle3 = %f \n", angle3);
platform.ui.console("cl1").printf("Angle2 = %f \n", angle2);
platform.ui.console("cl1").printf("Angle1 = %f \n", angle1);
platform.ui.console("cl1").printf("--------------------------\n");
hServoModule.s2.rotAbs(angle2);
} else if (i == 5) {
angle3 -= 2;
if (angle3 < -35) {
angle3 = -35;
}
sys.delay(30);
platform.ui.console("cl1").printf("Angle3 = %f \n", angle3);
platform.ui.console("cl1").printf("Angle2 = %f \n", angle2);
platform.ui.console("cl1").printf("Angle1 = %f \n", angle1);
platform.ui.console("cl1").printf("--------------------------\n");
hServoModule.s3.rotAbs(angle3);
} else if (i == 6) {
angle3 += 2;
if (angle3 > 82.5) {
angle3 = 82.5;
}
sys.delay(30);
platform.ui.console("cl1").printf("Angle3 = %f \n", angle3);
platform.ui.console("cl1").printf("Angle2 = %f \n", angle2);
platform.ui.console("cl1").printf("Angle1 = %f \n", angle1);
platform.ui.console("cl1").printf("--------------------------\n");
hServoModule.s3.rotAbs(angle3);
} else if (i == 7) {
angle4 -= 5;
if (angle4 < -90) {
angle4 = -90;
}
sys.delay(30);
platform.ui.console("cl2").printf("Angle4 = %f \n", angle4);
platform.ui.console("cl2").printf("Angle5 = %f \n", angle5);
platform.ui.console("cl2").printf("--------------------------\n");
hServoModule.s4.rotAbs(angle4);
} else if (i == 8) {
angle4 += 5;
if (angle4 > 90) {
angle4 = 90;
}
sys.delay(30);
platform.ui.console("cl2").printf("Angle4 = %f \n", angle4);
platform.ui.console("cl2").printf("Angle5 = %f \n", angle5);
platform.ui.console("cl2").printf("--------------------------\n");
hServoModule.s4.rotAbs(angle4);
} else if (i == 9) {
angle5 -= 5;
if (angle5 < -120) {
angle5 = -120;
}
sys.delay(30);
platform.ui.console("cl2").printf("Angle4 = %f \n", angle4);
platform.ui.console("cl2").printf("Angle5 = %f \n", angle5);
platform.ui.console("cl2").printf("--------------------------\n");
hServoModule.s5.rotAbs(angle5);
} else if (i == 10) {
angle5 += 5;
if (angle5 > -11) {
angle5 = -11;
}
sys.delay(30);
platform.ui.console("cl2").printf("Angle4 = %f \n", angle4);
platform.ui.console("cl2").printf("Angle5 = %f \n", angle5);
platform.ui.console("cl2").printf("--------------------------\n");
hServoModule.s5.rotAbs(angle5);
} else if (i == 11) {
mot1 = 10;
hMot1.rotRel(mot1);
sys.delay(30);
} else if (i == 12) {
mot1 = -10;
hMot1.rotRel(mot1);
sys.delay(30);
} else {
i = 0;
mot1 = 0;
}
}
}
void onKeyEvent(KeyEventType type, KeyCode code) //keys handling
{
switch (code) {
case KeyCode::Right: keyD = type == KeyEventType::Pressed; break;
case KeyCode::Down: keyS = type == KeyEventType::Pressed; break;
case KeyCode::Key_L:
if (type == KeyEventType::Pressed) {
i = 1;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_P:
if (type == KeyEventType::Pressed) {
i = 2;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_O:
if (type == KeyEventType::Pressed) {
i = 3;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_K:
if (type == KeyEventType::Pressed) {
i = 4;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_J:
if (type == KeyEventType::Pressed) {
i = 5;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_I:
if (type == KeyEventType::Pressed) {
i = 6;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_A:
if (type == KeyEventType::Pressed) {
i = 7;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_D:
if (type == KeyEventType::Pressed) {
i = 8;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_W:
if (type == KeyEventType::Pressed) {
i = 9;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_S:
if (type == KeyEventType::Pressed) {
i = 10;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_Z:
if (type == KeyEventType::Pressed) {
i = 11;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_X:
if (type == KeyEventType::Pressed) {
i = 12;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
default : break;
}
}
void hMain()
{
platform.begin(&RPi);
platform.ui.configHandler = cfgHandler;
platform.ui.onKeyEvent = onKeyEvent;
platform.ui.setProjectId("2026be4b06299d01");
sys.taskCreate(servoMotionSeparately, 2, 1024, 0);
sys.taskCreate(motorMotion, 2, 1024, 0);
sys.taskCreate(debugConsole_task, 2, 1000, 0);
sys.taskCreate(status_task, 2, 1000, 0);
hMot1.setSlewRate(0.055);
hMot2.setSlewRate(0.055);
hMot3.setSlewRate(0.055);
hMot4.setSlewRate(0.055);
hSens1.pin1.setOut();
hSens2.pin1.setOut();
hServoModule.enablePower();
sys.delay(50); // 50ms
LED2.toggle();
}
//Author: Piotr Paterek, Kraków 05-08.2016
#include "hFramework.h"
#include "hCloudClient.h"
float x;
float y;
float z;
float teta;
float alfa;
float beta;
float gamma1;
float delta;
float tetar;
float psi = -50;
float kappa = 0;
int m;
int mot1;
int i;
hPIDRegulator pidReg;
void debugConsole_task()
{
char ch;
platform.printf("Wpisz znak:\r\n");
for (;;) {
if (platform.read(&ch, 1) == 1) {
platform.printf("echo: %c\r\n", ch);
}
}
}
void status_task()
{
while (1) {
platform.ui.label("lb_bat").setText("%2f V", sys.getSupplyVoltage());
platform.ui.progressBar("pb_bat").setValue(sys.getSupplyVoltageMV() / 15); //supply voltage milivolts
sys.delay(200);
}
}
void cfgHandler()
{
auto l1 = platform.ui.label("l1");
auto l2 = platform.ui.label("l2");
auto b = platform.ui.button("btn1");
auto slider = platform.ui.input("slider1");
auto input = platform.ui.input("text");
platform.ui.loadHtml({Resource::WEBIDE, "/ui.html"});
// platform.ui.video.enable();
}
void onValueChangeEvent(hId id, const char* data)
{
LED1.toggle();
//platform.ui.console("cl1").printf("qq ");
if (id == "katTeta") {
teta = atoi(data);
platform.ui.console("cl2").printf("Gripper angle with base = %f \n", teta);
}
}
void motorMotion() //pid regulator for the motor
{
// hPID
pidReg.setScale(1);
pidReg.setKP(39.0);
pidReg.setKI(0.05);
pidReg.setKD(1000);
// hRegulator
pidReg.dtMs = 5;
pidReg.stableRange = 10;
pidReg.stableTimes = 3;
hMot1.attachPositionRegulator(pidReg);
}
void onKeyEvent(KeyEventType type, KeyCode code) //key handling
{
switch (code) {
case KeyCode::Key_P:
if (type == KeyEventType::Pressed) {
i = 1;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_L:
if (type == KeyEventType::Pressed) {
i = 2;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_O:
if (type == KeyEventType::Pressed) {
i = 3;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_K:
if (type == KeyEventType::Pressed) {
i = 4;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_A:
if (type == KeyEventType::Pressed) {
i = 5;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_D:
if (type == KeyEventType::Pressed) {
i = 6;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_W:
if (type == KeyEventType::Pressed) {
i = 7;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_S:
if (type == KeyEventType::Pressed) {
i = 8;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_Z:
if (type == KeyEventType::Pressed) {
i = 9;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_X:
if (type == KeyEventType::Pressed) {
i = 10;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
}
}
void servoMotionPath()
{
hServoModule.s1.calibrate(-90, 2400, 90, 800); //servo calibration
hServoModule.s2.calibrate(-90, 2400, 90, 800);
hServoModule.s3.calibrate(-90, 800, 90, 2400);
hServoModule.s4.calibrate(-90, 800, 90, 2400);
hServoModule.s5.calibrate(-90, 800, 90, 2400);
while (1) { //reversed kinematics calculations
sys.delay(10);
tetar = teta * M_PI / 180;
float r = y - 150.0 * cosf(tetar);
float h = x - 150.0 * sinf(tetar);
float d1 = sqrt(pow(h, 2) + pow(r, 2));
float betar = acosf((45000.0 - pow(d1, 2)) / (45000.0));
beta = betar;
float alfar = atanf(h / r) + acosf(d1 / 300.0);
alfa = (180 * (alfar - (M_PI / 2))) / M_PI; //first servo angle
float d2 = sqrt(27541.0 + 21300.0 * cosf(betar));
float gammar = acosf((17459.0 + pow(d2, 2)) / (300.0 * d2)) + acosf((pow(d2, 2) - 18779) / (122 * d2)) + alfar - (M_PI / 2);
gamma1 = (180.0 * (gammar)) / M_PI; // ksecond servo angle
float A = sqrt(25000.0 - 15000.0 * cosf(0.428 + alfar - tetar));
float kat1 = acosf((20000.0 + pow(A, 2)) / (300.0 * A));
float kat2 = M_PI - alfar - kat1;
float B = sqrt(pow(27, 2) + pow(A, 2) - 2 * 27 * A * cosf(kat2));
float kat3 = acosf((pow(B, 2) + pow(27, 2) - pow(A, 2)) / (54 * B));
float kat4 = acosf((-18779.0 + pow(B, 2)) / (122.0 * B));
delta = (M_PI - kat3 - kat4) * 180.0 / M_PI; // third servo angle
if (i == 1) { //servo angles with limits
if (x>129){
x=129 ;
}
x += 2;
hServoModule.s1.rotAbs(alfa);
hServoModule.s2.rotAbs(gamma1);
hServoModule.s3.rotAbs(delta);
platform.ui.console("cl1").printf("Delta = %f \n", delta);
platform.ui.console("cl1").printf("Gamma = %f \n", gamma1);
platform.ui.console("cl1").printf("Alfa = %f \n", alfa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 2) {
x -= 2;
hServoModule.s1.rotAbs(alfa);
hServoModule.s2.rotAbs(gamma1);
hServoModule.s3.rotAbs(delta);
platform.ui.console("cl1").printf("Delta = %f \n", delta);
platform.ui.console("cl1").printf("Gamma = %f \n", gamma1);
platform.ui.console("cl1").printf("Alfa = %f \n", alfa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 3) {
y += 2;
hServoModule.s1.rotAbs(alfa);
hServoModule.s2.rotAbs(gamma1);
hServoModule.s3.rotAbs(delta);
platform.ui.console("cl1").printf("Delta = %f \n", delta);
platform.ui.console("cl1").printf("Gamma = %f \n", gamma1);
platform.ui.console("cl1").printf("Alfa = %f \n", alfa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 4) {
y -= 2;
hServoModule.s1.rotAbs(alfa);
hServoModule.s2.rotAbs(gamma1);
hServoModule.s3.rotAbs(delta);
platform.ui.console("cl1").printf("Delta = %f \n", delta);
platform.ui.console("cl1").printf("Gamma = %f \n", gamma1);
platform.ui.console("cl1").printf("Alfa = %f \n", alfa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 5) {
if (kappa > 90) {
kappa = 90;
}
kappa += 2;
hServoModule.s4.rotAbs(kappa);
platform.ui.console("cl2").printf("Psi = %f \n", psi);
platform.ui.console("cl2").printf("Kappa = %f \n", kappa);
platform.ui.console("cl2").printf("--------------------------\n");
} else if (i == 6) {
if (kappa < -90) {
kappa = -90;
}
kappa -= 2;
hServoModule.s4.rotAbs(kappa);
platform.ui.console("cl2").printf("Psi = %f \n", psi);
platform.ui.console("cl2").printf("Kappa = %f \n", kappa);
platform.ui.console("cl2").printf("--------------------------\n");
} else if (i == 7) {
if (psi > -40) {
psi = -40;
}
psi += 2;
hServoModule.s5.rotAbs(psi);
platform.ui.console("cl2").printf("Psi = %f \n", psi);
platform.ui.console("cl2").printf("Kappa = %f \n", kappa);
platform.ui.console("cl2").printf("--------------------------\n");
} else if (i == 8) {
if (psi < -110) {
psi = -110;
}
psi -= 2;
hServoModule.s5.rotAbs(psi);
platform.ui.console("cl2").printf("Psi = %f \n", psi);
platform.ui.console("cl2").printf("Kappa = %f \n", kappa);
platform.ui.console("cl2").printf("--------------------------\n");
} else if (i == 9) { //motor control
mot1 = 10;
hMot1.rotRel(mot1);
sys.delay(30);
} else if (i == 10) {
mot1 = -10;
hMot1.rotRel(mot1);
sys.delay(30);
} else {
i = 0;
mot1 = 0;
}
}
}
void hMain()
{
// For connection via Android:
platform.begin(&RPi);
platform.ui.configHandler = cfgHandler;
platform.ui.onKeyEvent = onKeyEvent;
sys.setLogDev(&Serial);
platform.ui.onValueChangeEvent = onValueChangeEvent;
platform.ui.setProjectId("53321f290d15eede");
sys.taskCreate(servoMotionPath, 2, 1024, 0);
sys.taskCreate(motorMotion, 2, 1024, 0);
sys.taskCreate(debugConsole_task, 2, 1000, 0);
sys.taskCreate(status_task, 2, 1000, 0);
sys.delay(20);
hServoModule.enablePower();
}
//Author: Piotr Paterek, Kraków 05-08.2016
#include "hFramework.h"
#include "hCloudClient.h"
float x;
float y;
float z;
float teta = 0;
float alfa = 0;
float beta;
float gamma1 = 0;
float delta = 0;
float tetar;
float kappa = 0;
float psi = -50;
float z1;
int i;
int mot1;
hPIDRegulator pidReg;
void debugConsole_task()
{
char ch;
platform.printf("Wpisz znak:\r\n");
for (;;) {
if (platform.read(&ch, 1) == 1) {
platform.printf("echo: %c\r\n", ch);
}
}
}
void status_task()
{
while (1) {
platform.ui.label("lb_bat").setText("%2f V", sys.getSupplyVoltage());
platform.ui.progressBar("pb_bat").setValue(sys.getSupplyVoltageMV() / 15); //supply voltage milivolts
sys.delay(200);
}
}
void cfgHandler()
{
auto l1 = platform.ui.label("l1");
auto l2 = platform.ui.label("l2");
auto b = platform.ui.button("btn1");
auto slider = platform.ui.input("slider1");
auto input = platform.ui.input("text");
platform.ui.loadHtml({Resource::WEBIDE, "/ui.html"});
// platform.ui.video.enable();
}
void onValueChangeEvent(hId id, const char* data) // text box handling
{
LED1.toggle();
//platform.ui.console("cl1").printf("qq ");
if (id == "slider1") {
int val = atoi(data);
// platform.ui.console("cl1").printf("slider %s changed to %d \n", id.str(), val);
} else if (id == "wspX") {
x = atoi(data);
// platform.ui.console("cl2").printf("na %s jest wspolrzedna x = %f \n", id.str(), x);
} else if (id == "wspY") {
y = atoi(data);
// platform.ui.console("cl2").printf("na %s jest wspolrzedna y = %f \n", id.str(), y);
} else if (id == "wspZ") {
z = atoi(data);
// platform.ui.console("cl2").printf("na %s jest wspolrzedna z = %f \n", id.str(), z);
} else if (id == "katTeta") {
teta = atoi(data);
// platform.ui.console("cl2").printf("na %s jest wspolrzedna z = %f \n", id.str(), teta);
}
}
void onKeyEvent(KeyEventType type, KeyCode code) //keys handling
{
switch (code) {
case KeyCode::Key_A:
if (type == KeyEventType::Pressed) {
i = 5;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_D:
if (type == KeyEventType::Pressed) {
i = 6;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_W:
if (type == KeyEventType::Pressed) {
i = 7;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_S:
if (type == KeyEventType::Pressed) {
i = 8;
}
if (type == KeyEventType::Released) {
i = 0;
}
break;
case KeyCode::Key_Z:
if (type == KeyEventType::Pressed) {
i = 9;
}
break;
case KeyCode::Key_X:
if (type == KeyEventType::Pressed) {
i = 0;
}
break;
}
}
void servoMotionXYZ()
{
hServoModule.s1.calibrate(-90, 2400, 90, 800); //servo calibrations
hServoModule.s2.calibrate(-90, 2400, 90, 800);
hServoModule.s3.calibrate(-90, 800, 90, 2400);
hServoModule.s4.calibrate(-90, 800, 90, 2400);
hServoModule.s5.calibrate(-90, 800, 90, 2400);
while (1) { //reversed kinematics calculations
sys.delay(10);
tetar = teta * M_PI / 180;
float r = y - 150.0 * cosf(tetar);
float h = x - 150.0 * sinf(tetar);
float d1 = sqrt(pow(h, 2) + pow(r, 2));
float betar = acosf((45000.0 - pow(d1, 2)) / (45000.0));
beta = betar;
float alfar = atanf(h / r) + acosf(d1 / 300.0);
alfa = (180 * (alfar - (M_PI / 2))) / M_PI; //first servo angles
float d2 = sqrt(27541.0 + 21300.0 * cosf(betar));
float gammar = acosf((17459.0 + pow(d2, 2)) / (300.0 * d2)) + acosf((pow(d2, 2) - 18779) / (122 * d2)) + alfar - (M_PI / 2);
gamma1 = (180.0 * (gammar)) / M_PI; // second servo angles
float A = sqrt(25000.0 - 15000.0 * cosf(0.428 + alfar - tetar));
float kat1 = acosf((20000.0 + pow(A, 2)) / (300.0 * A));
float kat2 = M_PI - alfar - kat1;
float B = sqrt(pow(27, 2) + pow(A, 2) - 2 * 27 * A * cosf(kat2));
float kat3 = acosf((pow(B, 2) + pow(27, 2) - pow(A, 2)) / (54 * B));
float kat4 = acosf((-18779.0 + pow(B, 2)) / (122.0 * B));
delta = (M_PI - kat3 - kat4) * 180.0 / M_PI; // third servo angles
z1 = 8300*z/360;
if (i == 5) { //servo angles with limits
if (kappa > 90) {
kappa = 90;
}
kappa += 2;
hServoModule.s4.rotAbs(kappa);
platform.ui.console("cl1").printf("Psi = %f \n", psi);
platform.ui.console("cl1").printf("Kappa = %f \n", kappa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 6) {
if (kappa < -90) {
kappa = -90;
}
kappa -= 2;
hServoModule.s4.rotAbs(kappa);
platform.ui.console("cl1").printf("Psi = %f \n", psi);
platform.ui.console("cl1").printf("Kappa = %f \n", kappa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 7) {
if (psi > -40) {
psi = -40;
}
psi += 2;
hServoModule.s5.rotAbs(psi);
platform.ui.console("cl1").printf("Psi = %f \n", psi);
platform.ui.console("cl1").printf("Kappa = %f \n", kappa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 8) {
if (psi < -110) {
psi = -110;
}
psi -= 2;
hServoModule.s5.rotAbs(psi);
platform.ui.console("cl1").printf("Psi = %f \n", psi);
platform.ui.console("cl1").printf("Kappa = %f \n", kappa);
platform.ui.console("cl1").printf("--------------------------\n");
} else if (i == 9) { // motor handling
if (hMot1.getEncoderCnt() +5 < z1) {
if (1) {
sys.delay(10);
hMot1.rotAbs(hMot1.getEncoderCnt() + 5);
}
platform.ui.console("cl1").printf("Encoder ticks = %d \n", hMot1.getEncoderCnt());
} else if (hMot1.getEncoderCnt() - 5 > z1) {
if (1) {
sys.delay(10);
hMot1.rotAbs(hMot1.getEncoderCnt() - 5);
}
platform.ui.console("cl1").printf("Encoder ticks = %d \n", hMot1.getEncoderCnt());
} else if (hMot1.getEncoderCnt() == z1) {
hMot1.setPower(0);
} else {
}
}
}
}
void motor() //pid regulator for motor
{
// hPID
pidReg.setScale(1);
pidReg.setKP(39.0);
pidReg.setKI(0.05);
pidReg.setKD(1000);
// hRegulator
pidReg.dtMs = 5;
pidReg.stableRange = 10;
pidReg.stableTimes = 3;
hMot1.attachPositionRegulator(pidReg);
}
void onButtonEvent(hId id, ButtonEventType type) //button handling
{
UiButton b = platform.ui.button("btn1");
if (id == "btn1") {
if (type == ButtonEventType::Pressed) {
platform.ui.console("cl2").printf("Alfa = %f \n", alfa);
platform.ui.console("cl2").printf("Gamma = %f \n", beta);
platform.ui.console("cl2").printf("Delta = %f \n", gamma1);
platform.ui.console("cl2").printf("-------------------------\n");
hServoModule.s1.rotAbs(alfa);
hServoModule.s2.rotAbs(gamma1);
hServoModule.s3.rotAbs(delta);
}
}
}
void hMain()
{
// For connection via Android:
platform.begin(&RPi);
platform.ui.configHandler = cfgHandler;
platform.ui.onButtonEvent = onButtonEvent;
sys.setLogDev(&Serial);
platform.ui.onValueChangeEvent = onValueChangeEvent;
platform.ui.onKeyEvent = onKeyEvent;
platform.ui.setProjectId("69e9e8c852599cce");
sys.taskCreate(servoMotionXYZ, 2, 1024, 0);
sys.taskCreate(motor, 2, 1024, 0);
sys.taskCreate(debugConsole_task, 2, 1000, 0);
sys.taskCreate(status_task, 2, 1000, 0);
sys.delay(20);
hServoModule.enablePower();
}
Thanks to MeArm.
Comments