Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

Diff: main.cpp

Revision:
29:4b7f6f15cedf
Parent:
28:4f2e15118f13
Child:
30:6ace856e0ad7
--- 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) {