Diff: main.cpp

Revision:

9:4ee354663560

Parent:

8:54a7da09ccad

Child:

10:9e96d14d7034

--- a/main.cpp	Wed Oct 14 14:17:14 2015 +0000
+++ b/main.cpp	Wed Oct 14 15:09:23 2015 +0000
@@ -11,11 +11,12 @@
 Serial pc(USBTX,USBRX);
 
 ////////////////ENCODERS
-const float cpr=32;
-QEI encoder1(D12,D13,NC,cpr);/// maybe use Encoder in stead of QEI, because Encoder had setposition
-QEI encoder2(D10,D11,NC,cpr);
-const float PIE=3.14159265359;
-const float counttorad=(cpr/(2*PIE));
+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 encoder2(D10,D11,NC,cpr_sensor);
+const double PIE=3.14159265359;
+const float counttorad=((2*PIE)/cpr_shaft);
 
 /////////////////////////////////CALIBRATION (MODE)
 int modecounter=1;
@@ -24,7 +25,7 @@
 Ticker readbuttoncalibrate_ticker;
 const double readbuttoncalibrate_frequency=10;
 
-const double radpersec_calibrate=0.1;
+const double radpersec_calibrate=0.1*PIE;
 
 DigitalIn buttonR(D2);
 DigitalIn buttonL(D3);
@@ -33,10 +34,10 @@
 controlandadjust mycontroller; // make a controller
 //controller constants
 float Kp=0.5;
-float Ki=0.001;
+float Ki=0.01;
 float Kd=0.001;
 Ticker control_ticker;
-const double control_frequency=50;
+const double control_frequency=25;
 const double Ts_control=1.0/control_frequency;
 
 float error1_int=0;// storage variables for the errors
@@ -235,6 +236,14 @@
         if (changemodebutton==0) {
             changemode();
         }
+        if (scopedata_go==true) {
+            scopedata();
+            scopedata_go=false;
+        }
+        if (safetyandthreshold_go==true) {
+            safetyandthreshold();
+            safetyandthreshold_go=false;
+        }
         ///////////////////////////////////////////NORMAL RUNNING MODE
         if(modecounter==0) {
             if (switchedmode==true) {
@@ -243,18 +252,11 @@
                 pc.printf("Program running\n");//
                 switchedmode=false;
             }
-            if (scopedata_go==true) {
-                scopedata();
-                scopedata_go=false;
-            }
+
             if (filter_go==true) {
                 filtereverything();
                 filter_go=false;
             }
-            if (safetyandthreshold_go==true) {
-                safetyandthreshold();
-                safetyandthreshold_go=false;
-            }
             if (control_go==true) {
                 float error1=0;
                 float error2=0;
@@ -274,11 +276,21 @@
                 switchedmode=false;
             }
             if (control_go==true) {
-                float error1=(desired_angle[0]-counttorad*encoder1.getPulses()); // this is the error you want to use
-                float error2=0;
+                float error1=(desired_angle[0]-counttorad*encoder1.getPulses());;
+                float error2=0;// this is the error you want to use
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
             }
+            if (readbuttoncalibrate_go==true) {
+                if (buttonR.read()==0 && buttonL.read()==1) {
+                    desired_angle[0] += (radpersec_calibrate/readbuttoncalibrate_frequency);
+                    readbuttoncalibrate_go=false;
+                }
+                if (buttonR.read()==1 && buttonL.read()==0) {
+                    desired_angle[0] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
+                    readbuttoncalibrate_go=false;
+                }
+            }
         }
         ////////////////////////////////////////////CALIBRATE LEFT ARM
         if (modecounter==2) {
@@ -291,15 +303,6 @@
                 float error2=(desired_angle[1]-counttorad*encoder2.getPulses());// this is the error you want to use
                 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
-
-            }
-            if (scopedata_go==true) {
-                scopedata();
-                scopedata_go=false;
-            }
-            if (safetyandthreshold_go==true) {
-                safetyandthreshold();
-                safetyandthreshold_go=false;
             }
             if (readbuttoncalibrate_go==true) {
                 if (buttonR.read()==0 && buttonL.read()==1) {