Diff: main.cpp

Revision:

0:40052f5ca77b

Child:

1:dc683e88b44e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 05 15:47:58 2015 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+//#include "HIDScope.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+//#include "biquadFilter.h"
+#include "encoder.h"
+
+void keep_in_range(double * in, double min, double max);
+
+volatile bool looptimerflag;
+
+void setlooptimerflag(void);
+double keep_position_in_range(double * in, double min, double max);
+
+int main() {
+    //LOCAL VARIABLES 
+    /*Potmeter input*/
+    AnalogIn potmeter(A0);
+    Encoder motor1(D13,D12,false);
+    /* MODSERIAL to get non-blocking Serial*/
+    MODSERIAL pc(USBTX,USBRX);
+    /* PWM control to motor */
+    PwmOut pwm_motor(D5);
+    /* Direction pin */
+    DigitalOut motordir(D4);
+    /* variable to store setpoint in */
+    double setpoint;
+    /* variable to store pwm value in*/
+    double pwm_to_motor;
+    /* variable to store position of the motor in */
+    double position;
+    
+    //START OF CODE
+    
+    pc.printf("bla \n\r");
+    
+    /*Set the baudrate (use this number in RealTerm too! */
+    pc.baud(9600);
+    
+   /*Create a ticker, and let it call the     */
+   /*function 'setlooptimerflag' every 0.01s  */
+    Ticker looptimer;
+    
+    looptimer.attach(setlooptimerflag,0.01);      // calls the looptimer flag every 0.01s
+    
+    pc.printf("bla \n\r");
+    
+    motor1.setPosition(0);
+    
+    //INFINITE LOOP
+    
+    while(1) {
+        /* Wait until looptimer flag is true. */
+        while(looptimerflag != true);
+
+        looptimerflag = false;
+        
+        setpoint = 400;
+        
+        pc.printf("setpoint: %d, position motor %d \n\r", setpoint, motor1.getPosition());
+        
+        position=motor1.getPosition();
+        
+        keep_in_range(&position,-4200,4200);
+        
+        pc.printf("pwm before keep in range: %d \n\r", position);
+        
+        /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
+        pwm_to_motor = (setpoint - position)*.001;
+    
+
+        pc.printf("pwm before keep in range: %d \n\r", pwm_to_motor);
+        
+        keep_in_range(&pwm_to_motor, -1,1);
+
+        pc.printf("pwm after keep in range: %d \n\r", pwm_to_motor);
+
+        if(pwm_to_motor > 0)
+            motordir=1;
+        else
+            motordir=0; 
+        pwm_motor=(abs(pwm_to_motor));
+    }
+}
+
+
+// Keep in range function
+void keep_in_range(double * in, double min, double max)
+{
+    *in > min ? *in < max? : *in = max: *in = min;
+}
+
+// Looptimerflag function
+void setlooptimerflag(void)
+{
+    looptimerflag = true;
+}
\ No newline at end of file