Diff: main.cpp

Revision:

5:b4ec742aa7d4

Parent:

4:d385669b2f50

--- a/main.cpp	Tue Oct 17 09:31:20 2017 +0000
+++ b/main.cpp	Tue Oct 17 10:09:51 2017 +0000
@@ -35,26 +35,63 @@
 double m2_err_int = 0;
 const double motor2_gain = 2*PI;
 
+//________________________________________________________________
+//Kinematica
+
+//Motor offsets (kinematica implementation)
+int max_rangex = 800;
+int max_rangey = 500;
+int L1 = 450;
+int L2 = 490;
+float alphaoffset = 10;
+float betaoffset = 35;
+float x_offset = 0.0;
+float y_offset = 0.0;
+
+
+float getreferencepositionx(double potmeter){
+    float x_target = potmeter * max_rangex;
+    return x_target;
+}
+float getreferencepositiony(double potmeter){
+    float y_target = potmeter * max_rangey;
+    return y_target;
+}
+
+float getreferenceanglealpha(const double PI){
+    float x = getreferencepositionx(potmeter1);
+    float y = getreferencepositiony(potmeter2);
+    float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
+    float theta1 = asin(L2*sin(theta2)/sqrt(x*x +y*y) + atan(y/x));
+    float alpha = (05.*PI) - theta1;
+    return alpha;
+}
+
+float getreferenceanglebeta(const double PI){
+    float x = getreferencepositionx(potmeter1);
+    float y = getreferencepositiony(potmeter2);
+    float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
+    float theta1 = asin(L2*sin(theta2)/sqrt(x*x +y*y) + atan(y/x));
+    float alpha = (05.*PI) - theta1;
+    float beta = PI - alpha - theta2;
+    return beta;
+}     
+
+//________________________________________________________________
+
 
 //Servo
 Servo MyServo(D9);
 InterruptIn But1(D8);
 int k=0;
 
-
-float getreferenceangle(const double PI, double potmeter){
-    float max_angle = 2*PI;
-    float  reference_angle = max_angle * potmeter;
-    return reference_angle;    
-}
-
 double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) {
     e_int =+ Ts * error;
     return Kp * error + Ki * e_int ;
 }
 
 void motor1_control(){
-    double referenceangle1 = getreferenceangle(PI, potmeter1);
+    double referenceangle1 = getreferenceanglealpha(PI);
     double position1 = RAD_PER_PULSE * encoder1.getPosition();
     double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
     motor1 = fabs(magnitude1);
@@ -69,7 +106,7 @@
 
 
 void motor2_control(){
-    double referenceangle2 = getreferenceangle(PI, potmeter2);
+    double referenceangle2 = getreferenceanglebeta(PI);
     double position2 = RAD_PER_PULSE * encoder2.getPosition();
     double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
     motor2 = fabs(magnitude2);
@@ -82,7 +119,7 @@
     }
 }
 
-void servo_control (){
+/*void servo_control (){
     motor1.period(0.02f);
     motor2.period(0.02f);
     if (k==0){
@@ -95,11 +132,11 @@
         k=0;
         motor1.period_us(200);
     }        
-}
+}*/
 
 
 int main(){
-    But1.rise(&servo_control);
+    //But1.rise(&servo_control);
     motor1.period(0.0002f);
     motor2.period(0.0002f);
     MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);