Diff: main.cpp

Revision:

8:0bbbe93bd1c4

Parent:

7:697293226e5e

Child:

9:18e7a0273106

--- a/main.cpp	Thu Oct 23 10:47:35 2014 +0000
+++ b/main.cpp	Mon Oct 27 13:13:01 2014 +0000
@@ -2,7 +2,7 @@
 #include "encoder.h"
 #include "HIDScope.h"
 
-#define TSAMP 0.005
+#define TSAMP 0.001
 #define K_P (0.1)
 #define K_I (0.03  *TSAMP)
 #define K_D (0.001 /TSAMP)
@@ -17,7 +17,7 @@
 
 Serial pc(USBTX, USBRX);
 DigitalOut led1(LED_RED);
-HIDScope scope(2);
+HIDScope scope(3);
 
 //motor 25D
 Encoder motor1(PTD3,PTD5); //wit, geel
@@ -39,19 +39,49 @@
 void looper()
 {
     motordir1=0;
-    pwm_motor1.write(1);
+    pwm_motor1.write(0.06);
     scope.set(0, motor1.getPosition());
-    scope.set(1, motor2.getPosition());
+    scope.set(1, motor1.getSpeed());
+    scope.set(2, motor2.getPosition());
     scope.send();
 }
 
+/*void clamp(float * in, float min, float max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
+float pi(float setpoint, float measurement)
+{
+    float error;
+    float           out_p = 0;
+    static float    out_i = 0;
+    error  = setpoint - motor1.getPosition();
+    out_p  = error*K_P;
+    out_i += error*K_I;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+
+    return out_p + out_i;
+}*/
 
 int main()
 {
+    wait(1);
+    /*motordir1=0;
+    pwm_motor1.period_us(100);
+    pwm_motor1.write(1);
+    wait(0.034);
+    pwm_motor1.write(0);
+    wait(1);
+    motordir1=1;
+    pwm_motor1.write(1);
+    wait(0.034);
+    pwm_motor1.write(0);*/
     Ticker log_timer;
     Ticker timer;
     log_timer.attach(looper, TSAMP);
     timer.attach(&attime, 7);
+
     while(1) {
 
     }