Diff: main.cpp

Revision:

0:49f7a745f758

Child:

1:555373c1b9be

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 01 10:23:33 2018 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+
+//positie van start punten in counts
+double rotation_end_position=1;
+double tower_1_position=1;
+double tower_end_position=1;
+const int revcount = 25*8400;
+
+// 
+void home(){
+    //translation home
+    if (counts2 > ((tower_end_position - tower_1_position)/2){
+        translation_start(0,1);
+        }
+    else {    
+        translation_start(1,1);
+        }
+    if (counts2 > ((tower_end_position - tower_1_position)/2 - 100){
+        if (counts2 < ((tower_end_position - tower_1_position)/2 + 100){
+            translation_stop()
+            }
+        else{}
+        }
+    else{}
+    
+    //rotation home
+    rotation_start();
+    if ((counts1 - (revcount * (counts1 / revcounts))) > (rotation_end_position - 100)){ //check if motor 1 is in the right position within 100 counts range
+        if ((counts1 - (revcount * (counts1 / revcounts))) < (rotation_end_position + 100)){
+        rotation_stop();
+            }
+        else {}
+        }
+    else {}
+    }
+  
\ No newline at end of file