Diff: main.cpp

Revision:

1:d7299175a12e

Parent:

0:4141aef83f4b

Child:

2:c7369b41f7ae

--- a/main.cpp	Wed Nov 01 12:55:50 2017 +0000
+++ b/main.cpp	Thu Nov 02 08:57:41 2017 +0000
@@ -22,15 +22,18 @@
 DigitalOut M2_direction(D4);
 AnalogIn potmeter(A0);      //left pot
 AnalogIn potmeter2(A1);     //right pot
-InterruptIn button1(D2);       //hardware interrupt for stopping motors
-DigitalIn LimSW1(D1);
-DigitalIn LimSW2(D0);
-DigitalIn HomStart(D3);
-Ticker motor_update;
+InterruptIn button1(D2);       //hardware interrupt for stopping motors - right button
+DigitalIn LimSW1(D9);
+DigitalIn LimSW2(D8);
+DigitalIn HomStart(D3); // - left button
+
 BiQuad bqlowpass(0,    0.611,    0.611,  1,    0.222);
 //create scope objects - note: script won't run when HID usb port is not connected
 //HIDScope scope(5); //set # of channels
-Ticker scopeTimer;
+
+Ticker motor_update1;
+Ticker motor_update2;
+Ticker error_update;
 
 
 //-----------------variable decleration----------------
@@ -44,51 +47,63 @@
 
 double M1_home; 
 double M1_error_pos = 0;
-float M1_Kp = 0.1;
-float M1_Ki = 0.1;
-float M1_Kd = 0.1;
+float M1_Kp = 2;
+float M1_Ki = 4;
+float M1_Kd = 0;
 double M1_e_int=0;
 double M1_e_prior=0;
 
 double M2_home; 
 double M2_error_pos = 0;
-float M2_Kp = 0.1;
+float M2_Kp = 0.3;
 float M2_Ki = 0.1;
-float M2_Kd = 0.1;
+float M2_Kd = 0;
 double M2_e_int=0;
 double M2_e_prior=0;
 
 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(0);
+    M2_direction.write(1);
     
-    while (true){
-   
-        if (HomStart == 0){
-          M1_speed.write(0.1);
-          }  
- 
-        if (LimSW1 ==  1){
-            M1_speed.write(0);
-            M2_speed.write(0.1);
-            M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
-            }
-        if (LimSW2 == 1) {
-            M2_speed.write(0);
-            M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
-            }
+       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 == 1 && LimSW2 ==1) {
+        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;
-                }
-}
+        }
+       }
+
 }
 
  
@@ -121,6 +136,7 @@
      M1_direction.write(1);
      }
     else{M1_speed.write(0);}
+    pc.printf("Motor1 set speed = %f \n\r",set_speed);
  }
  
  void M2_control(){
@@ -158,13 +174,15 @@
     double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
     
     double q1 = M1_actual_pos;
-    double q2 = 3.1416-M1_acual_pos-M2_actual_pos //see drawing
+    double q2 = 3.1416-M1_actual_pos-M2_actual_pos; //see drawing
     
-    double M1_reference_pos = potmeter.read();
-    double M2_reference_pos = potmeter2.read();
+    //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 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 = M2_reference_pos - M2_actual_pos;
+    M2_error_pos = 0;
 }
  
 int main() {
@@ -173,19 +191,18 @@
 M1_speed.period(1.0/pwm_freq);
 M2_speed.period(1.0/pwm_freq);
 pc.baud(115200);
+pc.printf("starting homing function now. Push button to start procedure \n\r");
 //commence homing procedure
 homing_system();
+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
-motor_update.attach(&M1_control, &M2_control,0.01);
-//motor_update.attach(&M2_control,0.01);
-
+motor_update1.attach(&M1_control,0.01);
+motor_update2.attach(&M2_control,0.01);
+error_update.attach(&geterror,0.01);
 
-
-
-
-
-pc.printf("initialization complete \n\r");
+pc.printf("initialization complete - position control of motors now active\n\r");
     
     while(1){