Revision 2:c7369b41f7ae, committed 2017-11-02

Comitter:

CasperBerkhout

Date:

Thu Nov 02 10:13:07 2017 +0000

Parent:

1:d7299175a12e

Child:

3:455b79d42636

Commit message:

Smooth homing implemented using PI control

Changed in this revision

--- a/main.cpp	Thu Nov 02 08:57:41 2017 +0000
+++ b/main.cpp	Thu Nov 02 10:13:07 2017 +0000
@@ -42,8 +42,6 @@
 float reference_pos;
 
 
-float Arm1_home = 112.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
-float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
 
 double M1_home; 
 double M1_error_pos = 0;
@@ -55,56 +53,24 @@
 
 double M2_home; 
 double M2_error_pos = 0;
-float M2_Kp = 0.3;
-float M2_Ki = 0.1;
+float M2_Kp = 1;
+float M2_Ki = 1.5;
 float M2_Kd = 0;
 double M2_e_int=0;
 double M2_e_prior=0;
 
+double setpoint = 0;
+
+double M1_rel_pos;
+double M2_rel_pos;
+
 float Ts = 0.002; //500hz sample freq
 
 bool M1homflag;
 bool M2homflag;
 bool Homstartflag;
   
- void homing_system () {
-    LimSW1.mode(PullDown);
-    LimSW2.mode(PullDown);
-    M1_speed.write(0);
-    M2_speed.write(0);
-    M1_direction.write(0);
-    M2_direction.write(1);
-    
-       while(1){
-        if (HomStart.read() == 0){
-          M1_speed.write(0.4);
-          wait(0.5);
-          M2_speed.write(0.3);
-          pc.printf("starting M1 \n\r");
-        }
-        if(LimSW1.read() ==  1){
-          
-          M1_speed.write(0);
-          M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
-        }    
-        if (LimSW2.read() == 1) {
-          M2_speed.write(0);
-          
-          M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
-        }
-            
-        if (LimSW1.read() == 1 && LimSW2.read() ==1) {
-                pc.printf("Homing finished \n\r");
-                M1_speed.write(0);
-                M2_speed.write(0);
-                wait(0.5);
-                M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
-                M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
-                break;
-        }
-       }
 
-}
 
  
 void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
@@ -152,7 +118,56 @@
     else{M2_speed.write(0);}       
   }
  
+ void homing_system () {
+    LimSW1.mode(PullDown);
+    LimSW2.mode(PullDown);
+    M1_speed.write(0);
+    M2_speed.write(0);
+    M1_direction.write(0);
+    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(LimSW1.read() ==  1){
+          M1_error_pos = 0;
+          M1_speed.write(0);
+          M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
+        }    
+        if (LimSW2.read() == 1) {
+          M2_error_pos = 0;
+          M2_speed.write(0);
+          M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
+        }
+            
+        if (LimSW1.read() == 1 && LimSW2.read() ==1) {
+                pc.printf("Homing finished \n\r");
+                M1_speed.write(0);
+                M2_speed.write(0);
+                wait(0.5);
+                M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
+                M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
+                break;
+                //while(1); //stop after homing.
+        }
+       }
 
+}
  
  void scopesend(){
   
@@ -169,12 +184,15 @@
 void geterror(){ 
     double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
     double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
+
+    float Arm1_home = 122.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
+    float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
     
-    double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
-    double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
+    double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
+    double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
     
     double q1 = M1_actual_pos;
-    double q2 = 3.1416-M1_actual_pos-M2_actual_pos; //see drawing
+    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