Diff: main.cpp

Revision:

1:82a5126adc56

Parent:

0:1d02168661ec

--- a/main.cpp	Mon Oct 13 08:53:22 2014 +0000
+++ b/main.cpp	Tue Oct 14 13:11:03 2014 +0000
@@ -12,30 +12,107 @@
 #include "mbed.h"
 #include "encoder.h"
 #include "MODSERIAL.h"
-#include "HIDscope.h"
+#include "HIDScope.h"
+
+
+//MAXIMALE UITWIJKING BEPALEN
+/** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
+void keep_in_range(float * in, float min, float max);
 
-//POSITIE EN SNELHEID UITLEZEN OP PC
-int main()
+/** variable to show when a new loop can be started*/
+/** volatile means that it can be changed in an    */
+/** interrupt routine, and that that change is vis-*/
+/** ible in the main loop. */
+
+volatile bool looptimerflag;
+
+/** function called by Ticker "looptimer"     */
+/** variable 'looptimerflag' is set to 'true' */
+/** each time the looptimer expires.          */
+void setlooptimerflag(void)
 {
-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
-    */
+    looptimerflag = true;
+}
+
+int main() {
+    //LOCAL VARIABLES 
+    /*Potmeter input*/
+    AnalogIn potmeter(PTC2);
+    /* Encoder, using my encoder library */
+    /* First pin should be PTDx or PTAx  */
+    /* because those pins can be used as */
+    /* InterruptIn                       */
     Encoder motor1(PTD0,PTC9);
-    /*Use USB serial to send values*/
+    /* MODSERIAL to get non-blocking Serial*/
     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());
+    /* PWM control to motor */
+    PwmOut pwm_motor(PTA12);
+    /* Direction pin */
+    DigitalOut motordir(PTD3);
+    /* variable to store setpoint in */
+    float setpoint;
+    /* variable to store pwm value in*/
+    float pwm_to_motor;
+    //START OF CODE
+    
+    /*Set the baudrate (use this number in RealTerm too! */
+    pc.baud(230400);
+    
+   /*Create a ticker, and let it call the     */
+   /*function 'setlooptimerflag' every 0.01s  */
+    Ticker looptimer;
+    looptimer.attach(setlooptimerflag,0.01);  
+    
+    //INFINITE LOOP 
+    while(1) {
+        /* Wait until looptimer flag is true. */
+        /* '!=' means not-is-equal            */
+        while(looptimerflag != true);
+        /* Clear the looptimerflag, otherwise */
+        /* the program would simply continue  */
+        /* without waitingin the next iteration*/
+        looptimerflag = false;
+        
+        /* Read potmeter value, apply some math */
+        /* to get useful setpoint value         */
+        setpoint = (potmeter.read()-0.5)*2000;
+        
+        /* Print setpoint and current position to serial terminal*/
+        pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition());
+        
+        /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
+        pwm_to_motor = (setpoint - motor1.getPosition())*.001;
+        /* Coerce pwm value if outside range          */
+        /* Not strictly needed here, but useful       */
+        /* if doing other calculations with pwm value */
+        keep_in_range(&pwm_to_motor, -1,1);
+        
+        /* Control the motor direction pin. based on   */
+        /* the sign of your pwm value.      If your    */
+        /* motor keeps spinning when running this code */
+        /* you probably need to swap the motor wires,  */
+        /* or swap the 'write(1)' and 'write(0)' below */
+        if(pwm_to_motor > 0)
+            motordir.write(1);
+        else
+            motordir.write(0);
+        //WRITE VALUE TO MOTOR
+        /* Take the absolute value of the PWM to send */
+        /* to the motor. */  
+        pwm_motor.write(abs(pwm_to_motor));
     }
 }
 
+
+//coerces 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 keep_in_range(float * in, float min, float max)
+{
+    *in > min ? *in < max? : *in = max: *in = min;
+}
+
+
 //AANSTUREN MOTOR (POSITIE EN SNELHEID)
 #define TSAMP 0.01
 #define K_P (0.1)
@@ -127,3 +204,25 @@
     return out_p + out_i + out_d;
 }
 
+
+//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());
+    }
+}
\ No newline at end of file