Diff: main.cpp

Revision:

0:65ab9f79a4cc

diff -r 000000000000 -r 65ab9f79a4cc main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 19 09:31:16 2015 +0000
@@ -0,0 +1,78 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+
+DigitalOut  motor1_direction(D4);
+PwmOut      motor1_speed(D5);
+PwmOut      led(D9);
+DigitalIn   button_1(PTC6); //counterclockwise
+DigitalIn   button_2(PTA4); //clockwise
+Encoder     motor1(D12,D13);
+HIDScope    scope(1);
+
+
+const int pressed = 0;
+
+double H;
+double P;
+double D;
+
+
+void sethome(){
+    motor1.setPosition(0);
+    H = motor1.getPosition();
+}
+
+void move_motor1_ccw (){
+    motor1_direction = 0;
+    motor1_speed = 0.8;
+}
+
+void move_motor1_cw (){
+    motor1_direction = 1;
+    motor1_speed = 0.8;
+}
+
+void movetohome(){
+    P = motor1.getPosition();
+    D = (P - H);
+
+    if (D == 0){
+        motor1_speed = 0;
+    }
+    else if (D > 0){
+        move_motor1_cw();
+    }
+    else if (D < 0){
+        move_motor1_ccw();
+    }
+}
+
+void move_motor1()
+{
+    if (button_1 == pressed) {
+        move_motor1_cw ();
+    }
+    else if (button_2 == pressed) {
+            move_motor1_ccw ();
+        }
+    else movetohome();
+}
+
+void read_encoder1 ()    // aflezen van encoder via hidscope??
+{
+    scope.set(0,motor1.getPosition());
+    led.write(motor1.getPosition()/100.0);
+    scope.send();
+    wait(0.2f);
+}
+
+int main()
+{
+
+    sethome();
+    while (true) {
+        read_encoder1();
+        move_motor1();
+    }
+}
\ No newline at end of file