Diff: main.cpp

Revision:

17:72d3522165ac

Parent:

16:63320b8f79c2

Child:

18:1c3254a32fd1

diff -r 63320b8f79c2 -r 72d3522165ac main.cpp
--- a/main.cpp	Mon Oct 19 13:26:14 2015 +0000
+++ b/main.cpp	Mon Oct 19 14:21:46 2015 +0000
@@ -18,7 +18,7 @@
 ////////////////ENCODERS
 const float cpr_sensor=32;
 const float cpr_shaft=cpr_sensor*131;
-QEI encoder1(D12,D13,NC,cpr_sensor);/// maybe use Encoder in stead of QEI, because Encoder had setposition
+QEI encoder1(D13,D12,NC,cpr_sensor);/// maybe use Encoder in stead of QEI, because Encoder had setposition
 QEI encoder2(D10,D11,NC,cpr_sensor);
 const double PIE=3.14159265359;
 const float counttorad=((2*PIE)/cpr_shaft);
@@ -38,8 +38,6 @@
 DigitalIn buttonR(D2);
 DigitalIn buttonL(D3);
 
-
-
 //////////////////////////////////CONTROLLER
 controlandadjust mycontroller; // make a controller
 //controller constants
@@ -90,7 +88,7 @@
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
 float desired_position=0;
 float desired_angle[]= {0,0};
-float mm_per_sec_emg=0.1;// move the pod 50 mm per sec if muscle is flexed
+float mm_per_sec_emg=50;// 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
@@ -142,12 +140,9 @@
 //gather data and send to scope
 void scopedata()
 {
-    scope.set(0,desired_angle[0]);
-    scope.set(1,counttorad*encoder1.getPulses());
-    scope.set(2,mycontroller.motor1pwm());
-    scope.set(3,desired_angle[1]);
-    scope.set(4,counttorad*encoder2.getPulses());
-    scope.set(5,desired_position);
+    scope.set(0,desired_position);
+    scope.set(1,desired_angle[0]);
+    scope.set(2,desired_angle[1]);
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
@@ -219,8 +214,6 @@
     pc.scanf("%f", &filter_extragain);
 }
 
-
-
 const float schiethoek=0.35*PIE;
 const float schiettijd=0.5;
 void shoot() // THIS NEEDS ADJUSTMEND
@@ -283,39 +276,34 @@
     }
 }
 ///////////////////////////////////////////////READ BUTTON AND MOVE DESIRED POSITION
-void readbutton()
+void readsignalbutton()
 {
-    volatile bool go=true;
+    int buttonr=buttonR.read();
+    int buttonl=buttonL.read();
     //check if pod has to shoot
-    if (buttonR.read()==0 && buttonL.read()==0 &&go==true) {
+    if (buttonr==0 && buttonl==0) {
         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 ) {
+    } else if (buttonr==0 && buttonl==1) {
         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
+        if (desired_position>=maxdisplacement) {//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) {
+        // check if pod has to move to the left
+    } else if (buttonr==1 && buttonl==0) {
         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
+        if (desired_position<=(-1*maxdisplacement)) {//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;
     }
@@ -374,6 +362,10 @@
                 filtereverything();
                 filter_go=false;
             }
+            if (readsignal_go==true) {
+                readsignal();
+                readsignal_go=false;
+            }
             if (control_go==true) {
                 desired_angle[0]=anglepos.positiontoangle1(desired_position,y_start);
                 desired_angle[1]=anglepos.positiontoangle2(desired_position,y_start);
@@ -383,10 +375,7 @@
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
             }
-            if (readsignal_go==true) {
-                readsignal();
-                readsignal_go=false;
-            }
+
             valuechangebutton.fall(&valuechange);
         }
         ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
@@ -459,8 +448,17 @@
                 ledblink_go=false;
             }
             if (readsignal_go==true) {
-                readbutton();
-                readsignal_go==false;
+                readsignalbutton();
+                readsignal_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);
+                control_go=false;
             }
         }