Diff: main.cpp

Revision:

4:b4530fb376dd

Parent:

3:5f59cbe53d7d

Child:

5:b9d5d7311dac

--- a/main.cpp	Tue Oct 20 13:15:35 2015 +0000
+++ b/main.cpp	Wed Oct 21 11:50:15 2015 +0000
@@ -26,8 +26,8 @@
 #define SAMPLETIME_REGELAAR 0.005
 
 //define states
-enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
-uint8_t state = GOTOHOMEPOSITION;
+enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP};
+uint8_t state = HOME;
 
 // Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering
 const double g = 360; // Aantal graden 1 rotatie
@@ -75,32 +75,31 @@
 double D;
 
 
-void sethome(){
+void gethome(){
     H = motor1.getPosition();
 }
 
 void move_motor1_ccw (){
     motor1_direction = 0;
-    motor1_speed = 0.8;
+    motor1_speed = 1;
 }
 
 void move_motor1_cw (){
     motor1_direction = 1;
-    motor1_speed = 0.8;
+    motor1_speed = 1;
 }
 
 void movetohome(){
     P = motor1.getPosition();
-    D = (P - H);
- 
-    if (D >= -36 && D <= 36){
+    
+    if (P >= -28 && P <= 28){
         motor1_speed = 0;
     }
-    else if (D > 24){
+    else if (P > 24){
         motor1_direction = 1;
         motor1_speed = 0.1;
     }
-    else if (D < -24){
+    else if (P < -24){
         motor1_direction = 0;
         motor1_speed = 0.1;
     }
@@ -125,6 +124,10 @@
     wait(0.2f);
 }
 
+void print_position(){
+    pc.printf("move motor \n\r");
+    wait(0.2f);
+    }
 int main()
 {    
     while (true) {
@@ -132,12 +135,14 @@
         
     switch (state) {                //zorgt voor het in goede volgorde uitvoeren van de cases
         
-        case GOTOHOMEPOSITION:      //positie op 0 zetten voor arm 1
+        case HOME:      //positie op 0 zetten voor arm 1
          {
-            pc.printf("gotohomeposition\n\r");
+            pc.printf("home\n\r");
             read_encoder1();
-            sethome();
+            gethome();
+            pc.printf("Home-position is %f\n\r", H);
             state = MOVE_MOTOR;
+            wait(5);
             break;
         }
         
@@ -145,8 +150,8 @@
         {
             pc.printf("move_motor\n\r");
             while (state == MOVE_MOTOR){
-            pc.printf("whiletrue\n\r");
             move_motor1();
+            print_position();
             if (button_1 == pressed && button_2 == pressed){
             state = BACKTOHOMEPOSITION;
             }
@@ -168,26 +173,24 @@
                 regelaar_ticker_flag = false;
                 
                 pc.printf("pulsmotorposition1 %d", motor1.getPosition());
-                pc.printf("referentie %d\n\r", H);
+                pc.printf("referentie %f\n\r", H);
                 
-                if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){
-                motor1.setPosition(0);
-                H = motor1.getPosition();
+                if (P <=24 && P >= -24){
+                pc.printf("pulsmotorposition1 %d", motor1.getPosition());
+                pc.printf("referentie %f\n\r", H);
                 state = STOP;
+                pc.printf("Laatste positie %f\n\r", motor1.getPosition());
                 break;
                 }
             }
         }
         case STOP:
         {
-           const int einde = 1;
+           static int c;
            while(state == STOP){
             motor1_speed = 0;
-            if (einde == 1){
-            pc.printf("homepositie %d\n\r", H);
-            pc.printf("huidige positie %d\n\r", P);
+            if (c++ == 0){
             pc.printf("einde\n\r");
-            
             }
                }
             break;