Revision 6:9e8847a0492c, committed 2017-11-02

Comitter:

CasperBerkhout

Date:

Thu Nov 02 14:24:47 2017 +0000

Parent:

4:de0923cf6bcc

Child:

7:b0b15eb27de1

Commit message:

Directions correct and setpoint angles defined

Changed in this revision

--- a/main.cpp	Thu Nov 02 12:11:41 2017 +0000
+++ b/main.cpp	Thu Nov 02 14:24:47 2017 +0000
@@ -54,7 +54,7 @@
 double M2_home; 
 double M2_error_pos = 0;
 float M2_Kp = 1.5;
-float M2_Ki = 0.5;
+float M2_Ki = 1.5;
 float M2_Kd = 0;
 double M2_e_int=0;
 double M2_e_prior=0;
@@ -63,6 +63,7 @@
 
 double M1_rel_pos;
 double M2_rel_pos;
+double M2_reference_pos;
 
 float Ts = 0.002; //500hz sample freq
 
@@ -116,7 +117,8 @@
      M2_direction.write(1);
      }
     else{M2_speed.write(0);}       
-    pc.printf("M2 integral = %f\n\r", M2_e_int);
+    //pc.printf("M2 speed = %f, M2 direction = %i, M2 pos error =  %f, M2 setpoint = %f\n\r",set_speed,M2_direction.read(),M2_error_pos,M2_reference_pos);
+    //pc.printf("M2 integral = %f position error = %f\n\r", M2_e_int,M2_error_pos);
   }
  
  void homing_system () {
@@ -124,8 +126,7 @@
     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){
@@ -136,10 +137,10 @@
             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;  
             if(LimSW1.read() == 0){
-                  M1_error_pos = 1.5*setpoint - M1_rel_pos;
+                  M1_error_pos = 1.2*setpoint - M1_rel_pos;
             }
             if(LimSW2.read() == 0){
-                M2_error_pos = -(0.5*setpoint - M2_rel_pos);
+                M2_error_pos = - setpoint - M2_rel_pos;
             }
             M1_control();
             M2_control();
@@ -163,9 +164,10 @@
                 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.
+                break;
+                //while(1); //stop after homing.
         }
+        pc.printf("M2 error = %f M2 reference = %f\n\r", M2_error_pos,M2_reference_pos);
         wait(0.01);
        }
 
@@ -198,18 +200,19 @@
     
     //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-potmeter.read(); //should cover the right range - radians
-    double M2_reference_pos = 0.5+potmeter2.read()*0.25*3.1416;
+    //double M2_reference_pos = 0.25*3.1416+potmeter2.read();
+    double M2_reference_pos = 0.4+potmeter2.read();
         
     
-    //M2_error_pos = M2_reference_pos - M2_actual_pos;
-    M2_error_pos = 0;
+    pc.printf("M2 home = %f, M2 reference = %f \n\r",Arm2_home,M2_reference_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){
+    if(M2_reference_pos < Arm2_home){
         M2_reference_pos = Arm2_home;
     }
     else{
@@ -218,7 +221,7 @@
 }
  
 int main() {
-    
+button1.fall(StopMotors);      
     //initialize serial comm and set motor PWM freq
 M1_speed.period(1.0/pwm_freq);
 M2_speed.period(1.0/pwm_freq);
@@ -229,7 +232,7 @@
 pc.printf("Setting home position complete\n\r");
 //attach all interrupt
 pc.printf("attaching interrupt tickers now \n\r");
-button1.fall(StopMotors);     //stop motor interrupt
+   //stop motor interrupt
 motor_update1.attach(&M1_control,0.01);
 motor_update2.attach(&M2_control,0.01);
 error_update.attach(&geterror,0.01);