Diff: main.cpp

Revision:

0:73e1c3fd28b5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 05 09:35:05 2013 +0000
@@ -0,0 +1,244 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+
+/*******************************************************************************
+*                                                                              *
+*   Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/  *
+*                                                                              *
+********************************************************************************/
+
+/** 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);
+
+/** 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)
+{
+    looptimerflag = true;
+}
+
+
+int main()
+{
+    //LOCAL VARIABLES
+    /*Potmeter input*/
+    AnalogIn potmeterA(PTC2);
+    AnalogIn potmeterB(PTB2);
+    /* Encoder, using my encoder library */
+    /* First pin should be PTDx or PTAx  */
+    /* because those pins can be used as */
+    /* InterruptIn                       */
+    Encoder motorA(PTD4,PTC8);
+    Encoder motorB(PTD0,PTD2);
+    /* MODSERIAL to get non-blocking Serial*/
+    MODSERIAL pc(USBTX,USBRX);
+    /* PWM control to motor */
+    PwmOut pwm_motorA(PTA12);
+    PwmOut pwm_motorB(PTA5);
+    /* Direction pin */
+    DigitalOut motordirA(PTD3);
+    DigitalOut motordirB(PTD1);
+    /* variable to store setpoint in */
+    float setpointA;
+    float setpointB;
+    /* variable to store pwm value in*/
+    float pwm_to_motorA;
+    float pwm_to_motorB;
+    
+    /* variable to store setpoint in */
+        float setpoint_beginA;
+    float setpoint_beginB;
+    float setpoint_rechtsonderA;
+    float setpoint_rechtsonderB;
+
+    /* variable to store pwm value in*/
+    
+    float pwm_to_begin_motorA = 0;
+    float pwm_to_begin_motorB = 0;
+    
+    float pwm_to_rechtsonder_motorA;
+    float pwm_to_rechtsonder_motorB;
+    
+    /* variable to store positions in*/
+    int32_t positionmotorA_t0;
+    int32_t positionmotorB_t0;
+    int32_t positionmotorA_t_1;
+    int32_t positionmotorB_t_1;
+    int32_t positiondifference_motorA;
+    int32_t positiondifference_motorB;
+
+
+
+    //START OF CODE
+
+    /*Set the baudrate (use this number in RealTerm too! */
+    pc.baud(921600);
+    
+    motordirB.write(0);
+    pwm_motorB.write(.1);   //0.08
+    positionmotorB_t0 = motorB.getPosition();
+    do {
+        wait(0.2);
+        positionmotorB_t_1 = positionmotorB_t0 ;
+        positionmotorB_t0 = motorB.getPosition();
+        positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1);
+    } while(positiondifference_motorB > 10);
+    motorB.setPosition(0);
+    pwm_motorB.write(0);
+
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+
+    motordirA.write(1);
+    pwm_motorA.write(.1);
+    positionmotorA_t0 = motorA.getPosition();
+    do {
+        wait(0.2);
+        positionmotorA_t_1 = positionmotorA_t0 ;
+        positionmotorA_t0 = motorA.getPosition();
+        positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1);
+    } while(positiondifference_motorA > 10);
+    motorA.setPosition(0);
+    pwm_motorA.write(0);
+    
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+
+    // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links.
+
+    motordirA.write(0);
+    pwm_motorA.write(.08);
+    do {
+        setpoint_beginA = -63;      // x-as
+        pwm_to_begin_motorA = abs((setpoint_beginA + motorA.getPosition()) *.001);   // + omdat men met een negatieve hoekverdraaiing werkt.
+        wait(0.2);
+        keep_in_range(&pwm_to_begin_motorA, -1, 1 );
+        motordirA.write(0);
+        pwm_motorA.write(pwm_to_begin_motorA);
+    } while(pwm_to_begin_motorA <= 0);
+    motorA.setPosition(0);
+    pwm_motorA.write(0);
+
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+
+    // hierna moet motor A naar de rechtsonder A4. Motor A 532.
+
+    motordirA.write(0);
+    pwm_motorA.write(0.08);
+    do {
+        setpoint_rechtsonderA = -532;     // -532 rechtsonder positie A4
+        pwm_to_rechtsonder_motorA = abs((setpoint_rechtsonderA + motorA.getPosition()) *.001);
+        wait(0.2);
+        keep_in_range(&pwm_to_rechtsonder_motorA, -1, 1 );
+        motordirA.write(0);
+        pwm_motorA.write(pwm_to_rechtsonder_motorA);
+    } while(pwm_to_rechtsonder_motorA <= 0);
+    pwm_motorA.write(0);
+
+    wait(1);
+
+    // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan.
+
+    motordirB.write(1);
+    pwm_motorB.write(.08);
+    do {
+        setpoint_beginB = 192;      // x-as
+        pwm_to_begin_motorB = abs((setpoint_beginB - motorB.getPosition()) *.001);
+        wait(0.2);
+        keep_in_range(&pwm_to_begin_motorB, -1, 1 );
+        motordirB.write(1);
+        pwm_motorB.write(pwm_to_begin_motorB);
+    } while(pwm_to_begin_motorB <= 0);
+    motorB.setPosition(0);
+    pwm_motorB.write(0);
+
+    wait(1);            // willen nu even dat tussen ene actie en andere actie 1 seconde wacht.
+
+    // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 460.
+
+    motordirB.write(1);
+    pwm_motorB.write(0.08);
+    do {
+        setpoint_rechtsonderB = 460;      // rechtsonder positie A4
+        pwm_to_rechtsonder_motorB = abs((setpoint_rechtsonderB - motorB.getPosition()) *.001);
+        wait(0.2);
+        keep_in_range(&pwm_to_rechtsonder_motorB, -1, 1 );
+        motordirB.write(1);
+        pwm_motorB.write(pwm_to_rechtsonder_motorB);
+    } while(pwm_to_rechtsonder_motorB <= 0);
+    pwm_motorB.write(0);
+
+    wait(1);
+
+    /*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         */
+        setpointA = (potmeterA.read()-1)*(1000);     //631/2
+        setpointB = (potmeterB.read()-0.5)*(1000);  //871 dan rotatie langzamer maken als lager maakt.
+
+        /* Print setpoint and current position to serial terminal*/
+        pc.printf("s: %f, %d ", setpointA, motorA.getPosition());
+        pc.printf("s: %f, %d \n\r", setpointB, motorB.getPosition());
+        
+        /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
+       pwm_to_motorA = (setpointA - motorA.getPosition())*.001;
+       pwm_to_motorB = (setpointB - motorB.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_motorA, -0.1,0.1);
+        keep_in_range(&pwm_to_motorB, -0.1,0.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_motorA > 0)
+            motordirA.write(1);
+        else 
+            motordirA.write(0);
+        if(pwm_to_motorB > 0)
+            motordirB.write(1);
+        else 
+            motordirB.write(0);
+            
+        
+        //WRITE VALUE TO MOTOR
+        /* Take the absolute value of the PWM to send */
+        /* to the motor. */
+        pwm_motorA.write(abs(pwm_to_motorA));
+        pwm_motorB.write(abs(pwm_to_motorB));
+    }
+}
+
+
+//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;
+}