Diff: main.cpp

Revision:

12:02abb60385c8

Parent:

11:4f65e70290ac

--- a/main.cpp	Wed Oct 15 13:57:45 2014 +0000
+++ b/main.cpp	Thu Oct 16 10:41:21 2014 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "encoder.h"
 #include "HIDScope.h"
+#include "MODSERIAL.h" //voor print realterm
 
 #define TSAMP 0.01
 #define K_P (0.01)
@@ -17,6 +18,8 @@
 float potsamples[POT_AVG];
 HIDScope scope(2);
 
+MODSERIAL pc(USBTX,USBRX); //voor print realterm
+
 
 void setlooptimerflag(void)
 {
@@ -27,6 +30,7 @@
 
 int main()
 {
+    pc.baud(115200); //baudrate instellen voor realterm print
     //start Encoder
     Encoder motor1(PTD0,PTC9);
     // @param int_a Pin to be used as InterruptIn! Be careful, as not all pins on all platforms may be used as InterruptIn.
@@ -38,6 +42,11 @@
     DigitalOut motordir(PTD1);
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,TSAMP);
+    float der, prevpos, stop; 
+    der = 0;        //derivative 
+    prevpos = 0;    //previous result of motor1.getPosition()
+    stop = 0;       //after x amount of whiles while loop stops
+    wait(20);
     while(1) {
         float setpoint;
         float new_pwm;        
@@ -46,19 +55,27 @@
         //potmeter value: 0-1
         //setpoint = (potmeter.read()-.5)*500;        
         //new_pwm = (setpoint - motor1.getPosition())*.001; -> P action
-        new_pwm = 1; // DIT IS PUUR VOOR DE STEPFUNCTION
+        new_pwm = -1; // DIT IS PUUR VOOR DE STEPFUNCTION
         //new_pwm = pid(setpoint, motor1.getPosition());
         clamp(&new_pwm, -1,1);
         //scope.set(0, setpoint);
-        scope.set(1, new_pwm);
-        scope.set(2, motor1.getPosition());
+        scope.set(0, new_pwm);
+        scope.set(1, motor1.getPosition());
         // ch 1, 2 and 3 set in pid controller 
         scope.send();
         if(new_pwm > 0)
-            motordir = 0;
+            motordir = 0; // links 25D motor
         else
-            motordir = 1;
+            motordir = 1; // rechts 25D motor
         pwm_motor.write(abs(new_pwm));
+        
+       der = (motor1.getPosition() - prevpos)/TSAMP;
+       pc.printf("%f \n", der);
+       prevpos = motor1.getPosition();
+       stop++;
+       if (stop>500) {
+       break; 
+       }          
     }
 }