Diff: main.cpp

Revision:

4:d385669b2f50

Parent:

3:1514725c8cc8

Child:

5:b4ec742aa7d4

--- a/main.cpp	Mon Oct 16 12:15:09 2017 +0000
+++ b/main.cpp	Tue Oct 17 09:31:20 2017 +0000
@@ -35,48 +35,6 @@
 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);
@@ -84,11 +42,11 @@
 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;
@@ -124,7 +82,7 @@
     }
 }
 
-/*void servo_control (){
+void servo_control (){
     motor1.period(0.02f);
     motor2.period(0.02f);
     if (k==0){
@@ -137,7 +95,7 @@
         k=0;
         motor1.period_us(200);
     }        
-}*/
+}
 
 
 int main(){