Diff: main.cpp

Revision:

5:36df561f3ac1

Parent:

4:c22f3095b130

Child:

6:b69b9597d4fc

--- a/main.cpp	Sat Nov 01 21:00:25 2014 +0000
+++ b/main.cpp	Mon Nov 03 11:49:06 2014 +0000
@@ -7,7 +7,7 @@
 
 /*Het script bestaat uit drie delen: INCLUDE AND DEFINE, FUNCTIONS, MAINSCRIPT. Per gedeelte wordt uitgelegd wat gedaan wordt.
 We laten de beweging in fases lopen. Fase 1 werkt in dit script. Voor fase 2 kan de positie al worden ingesteld, de slagbeweging
-moet nog gemaakt worden. Dit gaan we doen met een PID regelaar die naar een snelheid toe regelt. 
+moet nog gemaakt worden. Dit gaan we doen met een PID regelaar die naar een snelheid toe regelt.
 */
 
 //INCLUDE AND DEFINE ALL
@@ -15,13 +15,10 @@
 #include "MODSERIAL.h"
 #include <iostream>
 #include "encoder.h"
+#include "HIDScope.h"
 
-#define K_P (0.1)
-#define K_I (0.1)
-#define K_D (0.0005 /TSAMP)
-#define TSAMP 0.001
-#define I_LIMIT 1.
-#define THRESHOLD 0.02
+#define THRESHOLD 0.04
+#define NOSAMPL 500
 
 #define K_P1 (0.004)
 #define K_I1 (0.00001*TSAMP1)
@@ -116,13 +113,26 @@
 armposition - Op basis van de emgsignalen wordt hier een positie ingesteld.
 armtopos - Regelt de arm naar de opgegeven positie toe.
 */
-
+//HIDScope scope(5);
 //GLOBAL FUNCTIOMS
+/*void viewer(){
+    scope.set(0,yave1);
+    scope.set(1,y1);
+    scope.set(2,yave2);
+    scope.set(3,y2);
+  scope.send();
+    }*/
+
 void clamp(float * in, float min, float max)
 {
 *in > min ? *in < max? : *in = max: *in = min;
 }
 
+void clampint(int * in, int min, int max)
+{
+*in > min ? *in < max? : *in = max: *in = min;
+}
+
 void setlooptimerflag(void)
 {
     looptimerflag = true;
@@ -240,8 +250,8 @@
     ysum2 = ysum2 + ylp2;
     n++;
 
-    if(n==500) {
-        yave2 = ysum2/500;
+    if(n==NOSAMPL) {
+        yave2 = ysum2/NOSAMPL;
         ysum2 = 0;
         n = 0;
     }
@@ -306,6 +316,7 @@
         rood = 1;
         groen = 0;
         cout<<"ga naar mode 2"<<endl;
+        wait(0.2);
     } else if (y1>0 && y2>0) {
         check=1;
     } else if (y1>0) {
@@ -330,12 +341,14 @@
             pwm.write(0.4);
             wait(0.04);
             pwm.write(0);
+            cout<<"links"<<endl;
             break;
         case 1:
             dir = 0;
             pwm.write(0.4);
             wait(0.04);
             pwm.write(0);
+            cout<<"rechts"<<endl;
             break;
         case 2:
             pwm.write(0);
@@ -356,22 +369,25 @@
     clamp(&out_i1,-I_LIMIT1,I_LIMIT1);
     prev_error = error;
     out  = out_i1+out_p+out_d;
+    cout<<"out: "<<out<<endl;
     return out;
 }
 
 
 
+
 uint8_t armposition (uint8_t y1, uint8_t y2)
 {
     static int stand=0;
     if (y1>0 && y2>0 && check>0) {
-        speeddone=1;
+        badjedone=2;
         check=0;
         rood = 0;
         groen = 1;
         cout<<"ga naar mode 1"<<endl;
     } else if (y1>0 && y2>0) {
         check=1;
+        cout<<"zo naar mode 1"<<endl;
     } else if (y1>0) {
         stand=stand+1;
         check=0;
@@ -381,10 +397,10 @@
         check=0;
         cout<<"stand "<<stand<<endl;
     } else {
-        stand = 3;
+        
         check =0;
     }
-
+    clampint(&stand,0,3);
     return stand;
 }
 
@@ -393,7 +409,7 @@
     int place=0;
     switch(pos1) {
         case 0:
-            place = pos1;
+            place = 0;
             break;
         case 1:
             place = 100;
@@ -408,10 +424,12 @@
             break;
     }
 
-    while((abs(place-encoder1.getPosition()) >6)|| (v1!= 0)) {
+    while((abs(place-encoder1.getPosition()) >50)|| (v1!=0)) {
 
         while(!looptimerflag);
         looptimerflag = false;
+        cout<<v1<<endl;
+        cout<<place-encoder1.getPosition()<<endl;
         pwm1 = pidposition(place,encoder1.getPosition());
 
         if(pwm1>0) {
@@ -426,6 +444,7 @@
     pwm1 =0;
 
     cout<<"place done"<<endl;
+    wait(1);
 }
 
 // MAIN SCRIPT
@@ -438,6 +457,7 @@
     pc.baud(115200);
     timer.attach(setlooptimerflag,TSAMP);
     wait(2);
+    cout<<"Begin programma"<<endl;
     while(1) {
         //Per TSAMP word EMG uitgelzen, gefilterd en gemiddeld
         while(!looptimerflag);
@@ -458,25 +478,35 @@
         hand van de positie waarin de arm in fase 2 is gezet. Zodra de arm de bal geslagen heeft moet de arm gereset worden en terug gaan naar
         fase 1.*/
 
-        if (teller==500) {
+        if (teller==NOSAMPL) {
             teller=0;
-            if (badjedone==0) {
-                badjestand=badje(y1,y2);
-                batposition(badjestand);
-            } else if (badjedone==1) {
-                if (speeddone==1) {
-                    armspeed=armstand+1;
-                    badjedone=0;
-                    speeddone=0;
-                    badjestand=1;
-                    armstand=0;
-                   } else {
+            switch(badjedone) {
+                case 0:
+                    cout<<"fase1"<<endl;
+                    badjestand=badje(y1,y2);
+                    batposition(badjestand);
+                    break;
+
+                case 1:
+                    cout<<"fase2"<<endl;
                     armstand=armposition(y1,y2);
                     cout<<"armstand "<<armstand<<endl;
-                    armtopos(armstand);
-                }
+                    
+                    //armtopos(armstand);
+                    //cout<<"badjedont: "<<badjedone<<endl;
+                    break;
+
+                case 2:
+                    cout<<"terug"<<endl;
+                    badjedone=0;
+                    break;
+                default:
+                    break;
             }
+
         }
     }
+   
+}
 
-}
\ No newline at end of file
+