Diff: main.cpp

Revision:

0:1d02168661ec

Child:

1:82a5126adc56

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 13 08:53:22 2014 +0000
@@ -0,0 +1,129 @@
+/***************************************/
+/*                                     */
+/*   BRONCODE GROEP 5, MODULE 9, 2014  */
+/*       *****-THE SLAP-******         */
+/*                                     */
+/* -Dominique Clevers                  */
+/* -Rianne van Dommelen                */
+/* -Daan de Muinck Keizer              */
+/* -David den Houting                  */
+/* -Marjolein Thijssen                 */
+/***************************************/
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+#include "HIDscope.h"
+
+//POSITIE EN SNELHEID UITLEZEN OP PC
+int main()
+{
+while(1) //Loop
+    {
+    /** Make encoder object.
+    * First pin should be on PTAx or PTDx because those pins can be used as InterruptIn
+    * Second pin can be any digital input
+    */
+    Encoder motor1(PTD0,PTC9);
+    /*Use USB serial to send values*/
+    MODSERIAL pc(USBTX,USBRX);
+    /*Set baud rate to 115200*/
+    pc.baud(115200);
+    while(1) { //Loop
+        /**Wait to prevent buffer overflow, and to keep terminal readable (not scrolling too fast)*/
+        wait(0.2);
+        /** print position (integer) and speed (float) to the PC*/
+        pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed());
+    }
+}
+
+//AANSTUREN MOTOR (POSITIE EN SNELHEID)
+#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
+#define M1_DIR PTC9
+#define M2_PWM PTA5
+#define M2_DIR PTA4
+
+//#define POT_AVG 50
+
+void clamp(float * in, float min, float max);
+float pid(float setpoint, float measurement);
+volatile bool looptimerflag;
+//float potsamples[POT_AVG];
+HIDScope scope(6);
+
+
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
+
+int main()
+{
+    AnalogIn potmeter(PTC2);
+    //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
+    Encoder motor1(PTD0,PTA13);
+    /*PwmOut to motor driver*/
+    PwmOut pwm_motor(M2_PWM);
+    //10kHz PWM frequency
+    pwm_motor.period_us(75);
+    DigitalOut motordir(M2_DIR);
+    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;
+}
+