Diff: main.cpp

Revision:

15:3a6074f3ceaf

Parent:

14:6ec84bfda0f3

Child:

16:bf4c5affd3e9

--- a/main.cpp	Wed Oct 08 12:56:20 2014 +0000
+++ b/main.cpp	Wed Oct 08 13:40:40 2014 +0000
@@ -1,12 +1,18 @@
 #include "mbed.h"
 #include "encoder.h"
+#include "MODSERIAL.h"
+
 #define TSAMP 0.01
 #define K_P (0.01)
 #define K_I (0  *TSAMP)
 #define K_D (0  /TSAMP)
 #define I_LIMIT 1.
 #define POT_AVG 50
+#define WACHT 1
+#define SLAAN 2
+#define TERUGKEREN 3
 
+MODSERIAL pc(USBTX,USBRX);
 void clamp(float * in, float min, float max);
 float pid(float setpoint, float measurement);
 //-------------------------------------------------------------------------------------------Input potentiometer-------------------------------
@@ -22,8 +28,9 @@
 
 int main()
 {
-    int toestand;
-    
+    int toestand = TERUGKEREN;
+    //----------------------------------------------------------------dit is de initiële toestand zodat hij begint met hoek 0--------------------------
+    float angle,v3=0.2,v2=0.1,v1=0.05,anglemax=1
     Encoder motor1(PTD0,PTC9);
     PwmOut pwm_motor(PTA5);
     pwm_motor.period_us(100);
@@ -36,28 +43,27 @@
         while(!looptimerflag);
         looptimerflag = false;
         //---------------------------------------------------------leest potentiometer (?), probably won't do what I want it to do ------------------
-        //---------------------------------------------------------int 6? ---------------------------------------------------------------------------
-        if(toestand==2)
+        //angle = ???? Encoder
+        if(toestand==SLAAN)
         {
-            if ( 4.5 < potmeter.read() && potmeter.read()< 6.0 && angle<maxangle)
+            if ( 0.75 < potmeter.read() && angle<anglemax) //angle<anglemax moet ergens anders staan (iets met toestand)
             {
                 setpoint = v3; //experimenteel bepaald, snelheid om bovenste goal te raken
             }
-            else if ( 3 < potmeter.read())
+            else if ( 0.50 < potmeter.read())
             {
                 setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
             }
-            else if (1.5 < potmeter.read())
+            else if (0.25 < potmeter.read())
             {
                 setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
             }
             else 
             {
-                setpoint = 0;
+                setpoint = 0;  // toestand = TERGUGKEREN?
             }
         }
-        // setpoint = (potmeter.read()-.5)*500;  
-        //----------------------------------------------------------------------new_pwm = getal?.....----------------------------------------------------
+        //----------------------------------------------------------------------new_pwm = getal----------------------------------------------------
         new_pwm = pid(setpoint, motor1.getPosition());
         clamp(&new_pwm, -1,1);
         //-------------------------------------------------------------------------------------output motor richting-------------------------------------
@@ -65,11 +71,11 @@
             motordir = 0;
         else
             motordir = 1;
-        //-------------------------------------------------------------------------------output motorsnelhied (wij willen 4 distinctive stappen)---------
         pwm_motor.write(abs(new_pwm));
-        if(angle>anglemax)
+        if(angle>anglemax && toestand=SLAAN)
         { 
-            toestand = terugkeren;
+            toestand = TERUGKEREN;
+            pc.printf ("Toestand = terugkeren!");
         }
     }