Diff: main.cpp

Revision:

16:63320b8f79c2

Parent:

15:17de575b7385

Child:

17:72d3522165ac

--- a/main.cpp	Mon Oct 19 09:32:44 2015 +0000
+++ b/main.cpp	Mon Oct 19 13:26:14 2015 +0000
@@ -3,6 +3,7 @@
 #include "HIDScope.h"
 #include "Biquad.h"
 #include "controlandadjust.h"
+#include "angleandposition.h"
 
 //info out
 HIDScope scope(6);
@@ -10,9 +11,9 @@
 const double scope_frequency=500;
 Serial pc(USBTX,USBRX);
 
-DigitalOut ledred(LED1);
-DigitalOut ledgreen(LED2);
-DigitalOut ledblue(LED3);
+DigitalOut ledred(LED_RED);
+DigitalOut ledgreen(LED_GREEN);
+DigitalOut ledblue(LED_BLUE);
 
 ////////////////ENCODERS
 const float cpr_sensor=32;
@@ -89,12 +90,15 @@
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
 float desired_position=0;
 float desired_angle[]= {0,0};
-float mm_per_sec_emg=50;// move the pod 50 mm per sec if muscle is flexed
+float mm_per_sec_emg=0.1;// move the pod 50 mm per sec if muscle is flexed
 float fieldwidth=473;
 float safetymarginfield=75; //adjustable, tweak for maximum but safe range
 float maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
 float rad_per_sec_emg=0.25*PIE;// THIS ONE IS NOT NESSECARY FOR ACTUAL PROGRAM
 
+angleandposition anglepos;
+float y_start=255;
+float y_punch=473;
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
               control_go=false,
@@ -143,7 +147,7 @@
     scope.set(2,mycontroller.motor1pwm());
     scope.set(3,desired_angle[1]);
     scope.set(4,counttorad*encoder2.getPulses());
-    scope.set(5,filteredsignal1);
+    scope.set(5,desired_position);
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
@@ -241,12 +245,13 @@
         pass++;
     }
     schiettimer.stop();
-    ledblue=0;
+    ledblue=1;
     //terug na schieten
     desired_angle[0]+=schiethoek;
     desired_angle[1]-=schiethoek;
 }
 
+////////////////////////////////////////////////////READ EMG AND MOVE DESIRED POSITION
 void readsignal()
 {
     //check if pod has to shoot
@@ -257,7 +262,7 @@
     } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
         led1=1;
         led2=0;
-        desired_position += (rad_per_sec_emg/readsignal_frequency);// move desiredposition right ADJUS TO MM IN LAST VERSEION
+        desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
         if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
             desired_position=maxdisplacement;
         } else {
@@ -267,7 +272,7 @@
     } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
         led1=0;
         led2=1;
-        desired_position -= (rad_per_sec_emg/readsignal_frequency);//move desiredposition left ADJUST TO MM IN FINAL VERSION
+        desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
         if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
             desired_position=(-1*maxdisplacement);
         } else {
@@ -276,8 +281,44 @@
     } else {
         led1=led2=0;
     }
-    desired_angle[0]=(desired_position);// REMOVE IN FINAL VERSION
-    desired_angle[1]=desired_position;//REMOVE IN FINAL VERSION
+}
+///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
+void readbutton()
+{
+    volatile bool go=true;
+    //check if pod has to shoot
+    if (buttonR.read()==0 && buttonL.read()==0 &&go==true) {
+        led1=led2=1;
+        shoot();
+        // check if pod has to move to the right
+        go=false;
+    } else if (buttonR.read()==0 && buttonL.read()==1 && go==true ) {
+        led1=1;
+        led2=0;
+        desired_position += (mm_per_sec_emg/readsignal_frequency);// move desiredposition right
+        if (desired_position>=maxdisplacement && go==true) {//check if the pod doesnt move too far and hit the edge
+            desired_position=maxdisplacement;
+            go=false;
+        } else {
+            desired_position=desired_position;
+            go=false;
+        }
+        go=false;
+    } else if (buttonR.read()==1 && buttonL.read()==0&&go==true) {
+        led1=0;
+        led2=1;
+        desired_position -= (mm_per_sec_emg/readsignal_frequency);//move desiredposition left
+        if (desired_position<=(-1*maxdisplacement) && go==true) {//check if the pod doesnt move too far and hit the edge
+            desired_position=(-1*maxdisplacement);
+            go=false;
+        } else {
+            desired_position=desired_position;
+            go=false;
+        }
+        go=false;
+    } else {
+        led1=led2=0;
+    }
 }
 
 void changemode()
@@ -293,7 +334,7 @@
     wait(1);
 }
 
-
+///////////////////////////////////////////////////MAIN
 
 int main()
 {
@@ -306,10 +347,6 @@
     readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
     ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
 
-    ledred=0;
-    ledgreen=0;
-    ledblue=0;
-
     while(1) {
         if (changemodebutton==0) {
             changemode();
@@ -328,8 +365,8 @@
                 encoder1.reset();
                 encoder2.reset();
                 pc.printf("Program running\n");//
-                ledgreen=1;
-                led1=led2=ledred=0;
+                ledgreen=0;
+                led1=led2=ledred=ledblue=1;
                 switchedmode=false;
             }
 
@@ -338,6 +375,9 @@
                 filter_go=false;
             }
             if (control_go==true) {
+                desired_angle[0]=anglepos.positiontoangle1(desired_position,y_start);
+                desired_angle[1]=anglepos.positiontoangle2(desired_position,y_start);
+
                 float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
                 float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
@@ -353,12 +393,13 @@
         if (modecounter==1) {
             if(switchedmode==true) {
                 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
+                led1=led2=ledred=0;
+                ledgreen=ledblue=1;
                 switchedmode=false;
-                ledred=1;
-                led1=led2=ledgreen=0;
             }
             if (ledblink_go==true) {
                 led1=!led1;
+                ledblink_go=false;
             }
             if (control_go==true) {
                 float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
@@ -381,12 +422,13 @@
         if (modecounter==2) {
             if(switchedmode==true) {
                 pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
-                ledred=1;
-                led1=led2=ledgreen=0;
+                led1=led2=ledred=0;
+                ledgreen=ledblue=1;
                 switchedmode=false;
             }
             if (ledblink_go==true) {
                 led2=!led2;
+                ledblink_go=false;
             }
             if (control_go==true) {
                 float error1=0;
@@ -405,12 +447,22 @@
                 }
             }
         }
+        ///////////////////////////////BUTTONCONTROLMODE
         if (modecounter==3) {
-            if (ledblink==true) {
+            if (switchedmode==true) {
+                led1=led2=0;
+                ledred=ledblue=1;
+                switchedmode=false;
+            }
+            if (ledblink_go==true) {
                 ledgreen=!ledgreen;
-                //buttoncontrolmode
+                ledblink_go=false;
             }
+            if (readsignal_go==true) {
+                readbutton();
+                readsignal_go==false;
+            }
+        }
 
-        }
     }
 }