Diff: main.cpp

Revision:

3:455b79d42636

Parent:

2:c7369b41f7ae

Child:

4:de0923cf6bcc

--- a/main.cpp	Thu Nov 02 10:13:07 2017 +0000
+++ b/main.cpp	Thu Nov 02 11:01:24 2017 +0000
@@ -47,7 +47,7 @@
 double M1_error_pos = 0;
 float M1_Kp = 2;
 float M1_Ki = 4;
-float M1_Kd = 0;
+float M1_Kd = 0.19;
 double M1_e_int=0;
 double M1_e_prior=0;
 
@@ -127,22 +127,21 @@
     M2_direction.write(1);
     
        while(1){
-        if (HomStart.read() == 0){
-          wait(0.01);
-          double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
-          double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; 
-          setpoint += 0.003; //move setpoint 0.2 radian per second (at 100hz)
-          
-          if(LimSW1.read() == 0){
-                M1_error_pos = 1.5*setpoint - M1_rel_pos;
-          }
-          if(LimSW2.read() == 0){
-                M2_error_pos = -(0.7*setpoint - M2_rel_pos);
-          }
-          M1_control();
-          M2_control();
-          
-          pc.printf("starting M1 \n\r");
+            if (HomStart.read() == 0){
+                  wait(0.01);
+                  double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
+                  double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; 
+                  setpoint += 0.003; //move setpoint 0.2 radian per second (at 100hz)
+                  
+            if(LimSW1.read() == 0){
+                  M1_error_pos = 1.5*setpoint - M1_rel_pos;
+            }
+            if(LimSW2.read() == 0){
+                M2_error_pos = -(0.5*setpoint - M2_rel_pos);
+            }
+            M1_control();
+            M2_control();
+            pc.printf("starting M1 \n\r");
         }
         if(LimSW1.read() ==  1){
           M1_error_pos = 0;
@@ -195,12 +194,24 @@
     double q2 = q1 + M2_actual_pos; //see drawing
     
     //double M1_reference_pos = 1+potmeter.read()*0.5*3.1416; //should cover the right range - radians
-    double M1_reference_pos = 0.5*3.1416; //should cover the right range - radians
+    double M1_reference_pos = 0.5*3.1416-potmeter.read(); //should cover the right range - radians
     double M2_reference_pos = 0.5+potmeter2.read()*0.25*3.1416;
         
-    M1_error_pos = M1_reference_pos - M1_actual_pos;
+    
     //M2_error_pos = M2_reference_pos - M2_actual_pos;
     M2_error_pos = 0;
+    if(M1_reference_pos > Arm1_home){
+        M1_reference_pos = Arm1_home;
+    }
+    else{
+        M1_error_pos = M1_reference_pos - M1_actual_pos;
+    }
+    if(M2_reference_pos > Arm2_home){
+        M2_reference_pos = Arm2_home;
+    }
+    else{
+        M2_error_pos = M2_reference_pos - M2_actual_pos;
+    }
 }
  
 int main() {