homeposition definieren

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of home_position by Rianne Bulthuis

Files at this revision

API Documentation at this revision

Comitter:
riannebulthuis
Date:
Tue Oct 20 13:15:35 2015 +0000
Parent:
2:866a8a9f2b93
Commit message:
motor stopt op gewenste positie. Echter blijven de regels op de putty nog wel doorlopen (pc.printf)

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 866a8a9f2b93 -r 5f59cbe53d7d main.cpp
--- a/main.cpp	Tue Oct 20 12:38:31 2015 +0000
+++ b/main.cpp	Tue Oct 20 13:15:35 2015 +0000
@@ -11,6 +11,9 @@
 DigitalIn   button_2(PTA4); //clockwise
 Encoder     motor1(D12,D13);
 HIDScope    scope(1);
+AnalogIn PotMeter1(A1);
+Ticker controller;
+Ticker ticker_regelaar;
 
 MODSERIAL   pc(USBTX, USBRX);
 volatile bool regelaar_ticker_flag;
@@ -21,12 +24,50 @@
 }
 
 #define SAMPLETIME_REGELAAR 0.005
-float referentie_arm1 = 0;
 
 //define states
-enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION};
+enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
 uint8_t state = GOTOHOMEPOSITION;
 
+// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
+const double g = 360; // Aantal graden 1 rotatie
+const double c = 4200; // Aantal counts 1 rotatie
+const double q = c/(g);
+
+//PI-controller constante
+const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
+const double motor1_Ki = 0.002; // Integrating gain m1.
+const double motor1_Ts = 0.01; // Time step m1
+double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
+
+// Reusable P controller
+double Pc (double error, const double Kp)
+{
+    return motor1_Kp * error;
+}
+
+// Measure the error and apply output to the plant
+void motor1_controlP()
+{
+    double referenceP1 = PotMeter1.read();
+    double positionP1 = q*motor1.getPosition();
+    double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp);
+}
+
+// Reusable PI controller
+double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int)
+{
+    err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
+    return motor1_Kp*error + motor1_Ki*err_int;
+} // De totale fout die wordt hersteld met behulp van PI control.
+
+void motor1_controlPI()
+{
+    double referencePI1 = PotMeter1.read();
+    double positionPI1 = q*motor1.getPosition();
+    double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
+}
+
 const int pressed = 0;
 
 double H;
@@ -35,7 +76,6 @@
 
 
 void sethome(){
-    motor1.setPosition(0);
     H = motor1.getPosition();
 }
 
@@ -52,15 +92,17 @@
 void movetohome(){
     P = motor1.getPosition();
     D = (P - H);
-
-    if (D > -5 && D < 5){
+ 
+    if (D >= -36 && D <= 36){
         motor1_speed = 0;
     }
-    else if (D > 0){
-        move_motor1_cw();
+    else if (D > 24){
+        motor1_direction = 1;
+        motor1_speed = 0.1;
     }
-    else if (D < 0){
-        move_motor1_ccw();
+    else if (D < -24){
+        motor1_direction = 0;
+        motor1_speed = 0.1;
     }
 }
 
@@ -121,18 +163,35 @@
             ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
             
             while(state == BACKTOHOMEPOSITION){
+                movetohome();
                 while(regelaar_ticker_flag != true);
                 regelaar_ticker_flag = false;
                 
                 pc.printf("pulsmotorposition1 %d", motor1.getPosition());
-                pc.printf("referentie %f\n\r", referentie_arm1);
+                pc.printf("referentie %d\n\r", H);
                 
                 if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){
-                    referentie_arm1 = 0;
+                motor1.setPosition(0);
+                H = motor1.getPosition();
+                state = STOP;
+                break;
                 }
-                
+            }
+        }
+        case STOP:
+        {
+           const int einde = 1;
+           while(state == STOP){
+            motor1_speed = 0;
+            if (einde == 1){
+            pc.printf("homepositie %d\n\r", H);
+            pc.printf("huidige positie %d\n\r", P);
+            pc.printf("einde\n\r");
+            
+            }
+               }
             break;
-        }
-    }
+            }
+}
 }
 }
\ No newline at end of file