Diff: main.cpp

Revision:

3:1514725c8cc8

Parent:

2:47ec66bbe8f9

Child:

4:d385669b2f50

--- a/main.cpp	Wed Oct 11 12:14:37 2017 +0000
+++ b/main.cpp	Mon Oct 16 12:15:09 2017 +0000
@@ -1,11 +1,12 @@
 #include "mbed.h"
 #include "encoder.h"
 #include "Servo.h"
+#include "FastPWM.h"
 
 Ticker MyControllerTicker1;
 Ticker MyControllerTicker2;
 const double PI = 3.141592653589793;
-const double RAD_PER_PULSE = 0.000476190;
+const double RAD_PER_PULSE = 0.000749425;
 const double CONTROLLER_TS = 0.01;
 
 
@@ -16,8 +17,8 @@
 DigitalIn ENC2B(D13);
 Encoder encoder1(D13,D12);
 AnalogIn potmeter1(A3);
-const double MOTOR1_KP = 0.5;
-const double MOTOR1_KI = 1.5;
+const double MOTOR1_KP = 15;
+const double MOTOR1_KI = 10;
 double m1_err_int = 0;
 const double motor1_gain = 2*PI;
 
@@ -29,22 +30,65 @@
 DigitalIn ENC1B(D11);
 Encoder encoder2(D10,D11);
 AnalogIn potmeter2(A4);
-const double MOTOR2_KP = 0.5;
-const double MOTOR2_KI = 1.5;
+const double MOTOR2_KP = 15;
+const double MOTOR2_KI = 10;
 double m2_err_int = 0;
 const double motor2_gain = 2*PI;
 
+//________________________________________________________________
+//Test kinematica
+
+//Motor offsets (kinematica implementation)
+float alphaoffset = 10;
+float betaoffset = 35;
+float x_target, y_target;
+float x, y;
+int max_rangex = 800;
+int max_rangey = 500;
+
+float L1 = 450;
+float L2 = 490;
+float x_offset = 0.0;
+float y_offset = 0.0;
+
+
+float getreferencepositionx(double potmeter){
+    x_target = potmeter * max_rangex;
+    return x_target;
+}
+float getreferencepositiony(double potmeter){
+    y_target = potmeter * max_rangey;
+    return y_target;
+}
+
+float getreferenceangle(){
+    x_target = getreferencepositionx(potmeter1);
+    y_target = getreferencepositiony(potmeter2);
+    float x = x_target - x_offset;
+    float y = y_target - y_offset;
+    float L3_ = sqrt(x*x + y*y);
+    float L3 = L3_;
+    
+    float beta = 3.1415 - acos(((L1*L1 + L2*L2 - L3*L3)/(2 * L1 * L2)));
+    beta =  beta * 180 / 3.1415;
+    
+    float alpha = (acos((L1*L1+L3*L3-L2*L2)/(2*L1*L3))) *180 / 3.1415;
+    float beta_ = beta - alpha;
+}
+
+//________________________________________________________________
+
 //Servo
 Servo MyServo(D9);
 InterruptIn But1(D8);
 int k=0;
 
 
-float getreferenceangle(const double PI, double potmeter){
+/*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;
@@ -80,31 +124,29 @@
     }
 }
 
-void servo_control (){
+/*void servo_control (){
+    motor1.period(0.02f);
+    motor2.period(0.02f);
     if (k==0){
         MyServo = 0;
         k=1;
+        motor1.period_us(200);
     }
     else{
         MyServo = 2;
         k=0;
-        }
-}
+        motor1.period_us(200);
+    }        
+}*/
 
 
 int main(){
-    //motor1.period(0.01);
+    But1.rise(&servo_control);
+    motor1.period(0.0002f);
+    motor2.period(0.0002f);
     MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
     MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
-    But1.rise(&servo_control);
+    
        
     while(1) {}   
-}
-
-//{while(1) 
-//    motor1_control(motor1_gain);
-//    wait(0.005f);
-//     motor2_control(motor2_gain);
-//     wait(0.005f);
-  //  }
-     
\ No newline at end of file
+}
\ No newline at end of file