Diff: main.cpp

Revision:

3:5f59cbe53d7d

Parent:

2:866a8a9f2b93

Child:

4:b4530fb376dd

--- 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