Hello everyone,
I'm a beginner. I'd like to move a 2 links robot (2R robot; R stands for rotation joint) according to a such inverse kinematics. Basically starting from the end-effector trajectory (a circle), i'd like to obtain the motor angles and move the robot. The motors are standard PWM servos. The inverse kinematic algorithm should be ok but it does not seem so for the rest of the code as my prototype does not work properly. I'm sure there are several errors somewhere, could you correct the code, i'll really appreciate that. Thanks a lot.
M.
#include <Servo.h>
const float pi = 3.141592653;
int servopin1 = 2;
int servopin2 = 4;
#define L1 10.00 // first limb;
#define L2 10.00 // second limb;
Servo servo1; //motor1 at the base
Servo servo2; //motor2
#define MIN_PULSE_WIDTH 544.0 // the shortest pulse sent to a servo
#define MAX_PULSE_WIDTH 2000.0 // the longest pulse sent to a servo
void setup()
{
servo1.attach(servopin1);
servo2.attach(servopin2);
pinMode(servopin1,OUTPUT);
pinMode(servopin2,OUTPUT);
servo_park();
Serial.begin( 9600 );
Serial.println("Start");
delay( 500 );
}
void loop()
{
circle();
}
//solve the inverse kinematic
void set_arm( float x, float y, float angle )
{
float c2 = (square(x) + square(y) - square(L1) - square(L2))/(2L1L2);
float s2 = sqrt(1 - square(c2));
float theta2 = (atan2(s2,c2))*180/pi;
float k1 = L1 + (L2 * c2);
float k2 = L2 * s2;
float theta1 = (atan2(y,x) - atan2(k2,k1))180/pi;
// an opportune correction
if (theta1<-50 && angle>180)
{
theta1 = theta1+2pi;
}
int uS1 = MIN_PULSE_WIDTH + theta1 * ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / 180.0;
int uS2 = MIN_PULSE_WIDTH + theta2 * ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / 180.0;
digitalWrite(servopin1,HIGH);
delayMicroseconds(uS1);
digitalWrite(servopin1,LOW);
digitalWrite(servopin2,HIGH);
delayMicroseconds(uS2);
digitalWrite(servopin2,LOW);
delay(20);
servo1.detach();
servo2.detach();
}
void circle()
{
#define RADIUS 10.0
#define X0 1
#define Y0 1
float xpoint,ypoint;
for( float angle_circle = 0.0; angle_circle < 360.0; angle_circle += 1.0 ) {
xpoint = X0+RADIUS * cos(angle_circlepi/180) ;
ypoint = Y0+RADIUS * sin(angle_circlepi/180) ;
set_arm( xpoint, ypoint,angle_circle);
delay( 1 );
}
}
void servo_park()
{
digitalWrite(servopin1,HIGH);
delayMicroseconds(1000);
digitalWrite(servopin1,LOW);
digitalWrite(servopin2,HIGH);
delayMicroseconds(1000);
digitalWrite(servopin2,LOW);
delay(1000);
return;
}