Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

Files at this revision

API Documentation at this revision

Comitter:
jessekaiser
Date:
Tue Oct 21 13:14:02 2014 +0000
Child:
1:97d6b160f708
Commit message:
Kleine motor draait :)

Changed in this revision

Encoder.lib Show annotated file Show diff for this revision Revisions of this file
HIDScope.lib Show annotated file Show diff for this revision Revisions of this file
MODSERIAL.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-dsp.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.lib	Tue Oct 21 13:14:02 2014 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/vsluiter/code/Encoder/#18b000b443af
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HIDScope.lib	Tue Oct 21 13:14:02 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/tomlankhorst/code/HIDScope/#e44574634162
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Tue Oct 21 13:14:02 2014 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/Hooglugt/code/MODSERIAL/#2e4e3795a093
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 21 13:14:02 2014 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "HIDScope.h"
+
+#define TSAMP 0.01
+#define K_P (0.1)
+#define K_I (0.03  *TSAMP)
+#define K_D (0.001 /TSAMP)
+#define I_LIMIT 1.
+
+#define M1_PWM PTC8 //blauw
+#define M1_DIR PTC9 //groen
+#define M2_PWM PTA5
+#define M2_DIR PTA4
+
+
+void clamp(float * in, float min, float max);
+float pid(float setpoint, float measurement);
+volatile bool looptimerflag;
+HIDScope scope(6);
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+int main()
+{
+    //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
+    Encoder motor1(PTD3,PTD5); //wit, geel
+    /*PwmOut to motor driver*/
+    PwmOut pwm_motor(M1_PWM); // PTC8, blauw
+    //10kHz PWM frequency
+    pwm_motor.period_us(75);
+    DigitalOut motordir(M1_DIR); //PTC9, groen
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,TSAMP);
+    while(1) {
+        int16_t setpoint;
+        float new_pwm;
+        /*wait until timer has elapsed*/
+        while(!looptimerflag);
+        looptimerflag = false; //clear flag
+        /*potmeter value: 0-1*/
+        setpoint = (potmeter.read()-.5)*500;        
+        /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
+        new_pwm = pid(setpoint, motor1.getPosition());
+        clamp(&new_pwm, -1,1);
+        scope.set(0, setpoint);
+        scope.set(4, new_pwm);
+        scope.set(5, motor1.getPosition());
+        // ch 1, 2 and 3 set in pid controller */
+        scope.send();
+        if(new_pwm < 0)
+            motordir = 0;
+        else
+            motordir = 1;
+        pwm_motor.write(abs(new_pwm));
+    }
+}
+
+
+//clamps value 'in' to min or max when exceeding those values
+//if you'd like to understand the statement below take a google for
+//'ternary operators'.
+void clamp(float * in, float min, float max)
+{
+    *in > min ? *in < max? : *in = max: *in = min;
+}
+
+
+float pid(float setpoint, float measurement)
+{
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+    static float    out_i = 0;
+    float           out_d = 0;
+    error  = setpoint-measurement;
+    out_p  = error*K_P;
+    out_i += error*K_I;
+    out_d  = (error-prev_error)*K_D;
+    clamp(&out_i,-I_LIMIT,I_LIMIT);
+    prev_error = error;
+    scope.set(1,out_p);
+    scope.set(2,out_i);
+    scope.set(3,out_d);
+    return out_p + out_i + out_d;
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dsp.lib	Tue Oct 21 13:14:02 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-dsp/#9cee975aadce
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Oct 21 13:14:02 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/552587b429a1
\ No newline at end of file