Diff: main.cpp

Revision:

0:c205dc094877

Child:

2:302174acdac7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 23 11:58:51 2014 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+
+
+
+HIDScope scope(3);
+
+int main()
+{
+    //motor 1, 25D
+    Encoder motor1(PTD3, PTD5);
+    DigitalOut motor1dir(PTC9);
+    PwmOut pwm_motor1(PTC8);
+    pwm_motor1.period_us(100); //10kHz PWM frequency
+
+    //motor 2, 25D
+    Encoder motor2(PTD2,PTD0);
+    DigitalOut motor2dir(PTA4);
+    PwmOut pwm_motor2(PTA5);
+    pwm_motor2.period_us(100); //10kHz PWM frequency
+
+
+    integral1 = 0;
+    setpoint1 = 9.5;
+    dt1 = 0.01;
+    Kp1 = 0.01;
+    Ki1 = 0.01;
+    while(1) {
+start:
+        error1 = setpoint1 - motor1.getposition/(4480*6.28)/dt1; //motorpositie omgerekend naar rad/s
+        integral1 = integral1 + error1*dt1;
+        output1 = Kp1*error1 + Ki1*integral1;
+        pwm_motor1.write(abs(output1);
+                         wait(dt1)
+                         goto start
+
+        if(new_pwm > 0) {
+        motor1dir = 0;
+    } else {
+        motor1dir = 1;
+    }
+
+    scope.set(0, new_pwm);
+    scope.set(1, motor1.getPosition());
+    scope.set(2, motor1.getposition()/(4480*6.28)/dt1);
+    scope.send();
+}
+}
\ No newline at end of file