Diff: main.cpp

Revision:

29:4b7f6f15cedf

Parent:

28:4f2e15118f13

Child:

30:6ace856e0ad7

diff -r 4f2e15118f13 -r 4b7f6f15cedf main.cpp
--- a/main.cpp	Fri Oct 31 13:23:51 2014 +0000
+++ b/main.cpp	Fri Oct 31 13:26:13 2014 +0000
@@ -208,6 +208,23 @@
         } else {
             motordir1 = 1;
         }
+        
+         //MOTOR2
+        cur_pos_motor2 = motor2.getPosition();
+        pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI)); //moet 4128
+        pwm2_percentage = pid1(setpoint2, pos_motor2_rad);
+        if (pwm2_percentage < -1.0) {
+            pwm2_percentage = -1.0;
+        }
+        if (pwm2_percentage > 1.0) {
+            pwm2_percentage = 1.0;
+        }
+        pwm_motor2.write(abs(pwm2_percentage));
+        if(pwm2_percentage > 0) {
+            motordir2 = 0;
+        } else {
+            motordir2 = 1;
+        }
         wait(TSAMP);
         //STATES
         if(staat == 0) {