Diff: main.cpp

Revision:

2:866a8a9f2b93

Parent:

1:7d5e6bc2b314

Child:

3:5f59cbe53d7d

--- a/main.cpp	Mon Oct 19 13:44:23 2015 +0000
+++ b/main.cpp	Tue Oct 20 12:38:31 2015 +0000
@@ -12,6 +12,20 @@
 Encoder     motor1(D12,D13);
 HIDScope    scope(1);
 
+MODSERIAL   pc(USBTX, USBRX);
+volatile bool regelaar_ticker_flag;
+
+void setregelaar_ticker_flag()
+{
+    regelaar_ticker_flag = true;
+}
+
+#define SAMPLETIME_REGELAAR 0.005
+float referentie_arm1 = 0;
+
+//define states
+enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION};
+uint8_t state = GOTOHOMEPOSITION;
 
 const int pressed = 0;
 
@@ -39,7 +53,7 @@
     P = motor1.getPosition();
     D = (P - H);
 
-    if (D == 0){
+    if (D > -5 && D < 5){
         motor1_speed = 0;
     }
     else if (D > 0){
@@ -70,30 +84,55 @@
 }
 
 int main()
-{
+{    
     while (true) {
         pc.baud(9600);          //pc baud rate van de computer
         
     switch (state) {                //zorgt voor het in goede volgorde uitvoeren van de cases
         
-        case gotohomeposition:      //positie op 0 zetten voor arm 1
+        case GOTOHOMEPOSITION:      //positie op 0 zetten voor arm 1
          {
+            pc.printf("gotohomeposition\n\r");
             read_encoder1();
             sethome();
-            state = move_motor;
+            state = MOVE_MOTOR;
             break;
         }
         
-        case move_motor:            //motor kan cw en ccw bewegen a.d.h.v. buttons
+        case MOVE_MOTOR:            //motor kan cw en ccw bewegen a.d.h.v. buttons
         {
-            move_motor1 ();
-            state = homeposition;
-            break;
+            pc.printf("move_motor\n\r");
+            while (state == MOVE_MOTOR){
+            pc.printf("whiletrue\n\r");
+            move_motor1();
+            if (button_1 == pressed && button_2 == pressed){
+            state = BACKTOHOMEPOSITION;
+            }
+            }
+            wait (1);
+            break; 
         }
         
-        case backtohomeposition:    //motor gaat terug naar homeposition
+        case BACKTOHOMEPOSITION:    //motor gaat terug naar homeposition
         {
-            movetohome();
+            pc.printf("backhomeposition\n\r");
+            wait (1);
+            
+            ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR);
+            
+            while(state == BACKTOHOMEPOSITION){
+                while(regelaar_ticker_flag != true);
+                regelaar_ticker_flag = false;
+                
+                pc.printf("pulsmotorposition1 %d", motor1.getPosition());
+                pc.printf("referentie %f\n\r", referentie_arm1);
+                
+                if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){
+                    referentie_arm1 = 0;
+                }
+                
             break;
         }
+    }
+}
 }
\ No newline at end of file