Diff: main.cpp

Revision:

6:6ae6256cf234

Parent:

5:b4ec742aa7d4

--- a/main.cpp	Tue Oct 17 10:09:51 2017 +0000
+++ b/main.cpp	Thu Oct 19 09:10:20 2017 +0000
@@ -2,12 +2,15 @@
 #include "encoder.h"
 #include "Servo.h"
 #include "FastPWM.h"
+#include "MODSERIAL.h"
+
+MODSERIAL pc(USBTX, USBRX);
 
 Ticker MyControllerTicker1;
 Ticker MyControllerTicker2;
 const double PI = 3.141592653589793;
-const double RAD_PER_PULSE = 0.000749425;
-const double CONTROLLER_TS = 0.01;
+const double RAD_PER_PULSE = 0.00074799825;
+const double CONTROLLER_TS = 0.01; //0.01
 
 
 //Motor1
@@ -17,7 +20,7 @@
 DigitalIn ENC2B(D13);
 Encoder encoder1(D13,D12);
 AnalogIn potmeter1(A3);
-const double MOTOR1_KP = 15;
+const double MOTOR1_KP = 20;
 const double MOTOR1_KI = 10;
 double m1_err_int = 0;
 const double motor1_gain = 2*PI;
@@ -30,7 +33,7 @@
 DigitalIn ENC1B(D11);
 Encoder encoder2(D10,D11);
 AnalogIn potmeter2(A4);
-const double MOTOR2_KP = 15;
+const double MOTOR2_KP = 20;
 const double MOTOR2_KI = 10;
 double m2_err_int = 0;
 const double motor2_gain = 2*PI;
@@ -39,14 +42,14 @@
 //Kinematica
 
 //Motor offsets (kinematica implementation)
-int max_rangex = 800;
-int max_rangey = 500;
+int max_rangex = 400;
+int max_rangey = 250;
 int L1 = 450;
 int L2 = 490;
-float alphaoffset = 10;
-float betaoffset = 35;
-float x_offset = 0.0;
-float y_offset = 0.0;
+float alphaoffset = 0.577872;
+float betaoffset = -0.165529+0.577872; //21.52;
+float x_offset = 0.0;//165.07;
+float y_offset = 0.0;//106.37;
 
 
 float getreferencepositionx(double potmeter){
@@ -58,22 +61,22 @@
     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;
+float getreferenceanglealpha(const double PI, float x_offset, float y_offset, float alphaoffset){
+    float x = getreferencepositionx(potmeter1) + x_offset;
+    float y = getreferencepositiony(potmeter2) - y_offset;
+    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 = (0.5*PI) - theta1 - alphaoffset;
     return alpha;
 }
 
-float getreferenceanglebeta(const double PI){
-    float x = getreferencepositionx(potmeter1);
-    float y = getreferencepositiony(potmeter2);
+float getreferenceanglebeta(const double PI, float x_offset, float y_offset, float betaoffset, float alphaoffset){
+    float x = getreferencepositionx(potmeter1) + x_offset;
+    float y = getreferencepositiony(potmeter2) - y_offset;
     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;
+    float theta1 = asin( (L2*sin(theta2)) / sqrt(x*x +y*y)) + atan(y/x);
+    float alpha = (0.5*PI) - theta1 - alphaoffset;
+    float beta = PI - alpha - theta2 - betaoffset;
     return beta;
 }     
 
@@ -91,7 +94,7 @@
 }
 
 void motor1_control(){
-    double referenceangle1 = getreferenceanglealpha(PI);
+    double referenceangle1 = getreferenceanglebeta(PI, x_offset, y_offset, betaoffset, alphaoffset);
     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);
@@ -106,7 +109,7 @@
 
 
 void motor2_control(){
-    double referenceangle2 = getreferenceanglebeta(PI);
+    double referenceangle2 = getreferenceanglealpha(PI, x_offset, y_offset, alphaoffset);
     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);
@@ -136,12 +139,28 @@
 
 
 int main(){
+    
+    pc.baud(115200);
+    pc.printf("Hello World!\r\n");
+
+    
     //But1.rise(&servo_control);
     motor1.period(0.0002f);
     motor2.period(0.0002f);
     MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
     MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
     
+    
+    const double PI = 3.141592653589793; 
+    float x_offset = 165.07;
+    float y_offset = 106.37; 
+    float alphaoffset = 0.577872;
+    float betaoffset = -0.165529+0.577872; //21.52;
+    float alpha = getreferenceanglealpha(PI, x_offset, y_offset, alphaoffset);
+    pc.printf("a %f\n", alpha);
+ 
+    float beta = getreferenceanglebeta(PI, x_offset, y_offset, betaoffset, alphaoffset);
+    pc.printf("b %f\n", beta);
        
     while(1) {}   
 }
\ No newline at end of file