Diff: main.cpp

Revision:

7:7fbb2c028778

Parent:

6:37c94a5e205f

Child:

8:54a7da09ccad

--- a/main.cpp	Wed Oct 14 13:00:50 2015 +0000
+++ b/main.cpp	Wed Oct 14 13:42:28 2015 +0000
@@ -11,13 +11,19 @@
 Serial pc(USBTX,USBRX);
 
 ////////////////ENCODERS
-QEI encoder1(D12,D13,NC,32);/// maybe use Encoder in stead of QEI, because Encoder had setposition
-QEI encoder2(D10,D11,NC,32);
+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));
 
 /////////////////////////////////CALIBRATION (MODE)
 int modecounter=0;
 DigitalIn changemodebutton(PTA4);
 
+Ticker readbuttoncalibrate_ticker;
+const double readbuttoncalibrate_frequency=10;
+
 //////////////////////////////////CONTROLLER
 controlandadjust mycontroller; // make a controller
 //controller constants
@@ -26,11 +32,12 @@
 float Kd=0.001;
 Ticker control_ticker;
 const double control_frequency=25;
+const double Ts_control=1.0/control_frequency;
 
-double error1_int=0;// storage variables for the errors
-double error2_int=0;
-double error1_prev=0;
-double error2_prev=0;
+float error1_int=0;// storage variables for the errors
+float error2_int=0;
+float error1_prev=0;
+float error2_prev=0;
 
 InterruptIn valuechangebutton(PTC6);//button to change controller constants
 
@@ -74,7 +81,8 @@
               filter_go=false,
               safetyandthreshold_go=false,
               readsignal_go=false,
-              switchedmode=false;
+              switchedmode=false,
+              readbuttoncalibrate_go=false;
 
 void scopedata_activate()
 {
@@ -96,7 +104,10 @@
 {
     readsignal_go=true;
 }
-
+void readbuttoncalibrate_activate()
+{
+    readbuttoncalibrate_go=true;
+}
 
 ////////////////////////FUNCTIONS
 //gather data and send to scope
@@ -156,10 +167,7 @@
     //filter_timer.stop();
 }
 
-void control()
-{
-    ///call desired controller here
-}
+
 
 //adjust controller values when sw2 is pressed
 void valuechange()
@@ -205,6 +213,8 @@
     wait(1);
 }
 
+
+
 int main()
 {
     //tickers
@@ -213,12 +223,14 @@
     control_ticker.attach(&control_activate,1.0/control_frequency);
     scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
     readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
+    readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
 
     while(1) {
         if (changemodebutton==0) {
             changemode();
         }
-        if(modecounter==0) {//normal running mode
+        ///////////////////////////////////////////NORMAL RUNNING MODE
+        if(modecounter==0) {
             if (switchedmode==true) {
                 encoder1.reset();
                 encoder2.reset();
@@ -238,7 +250,9 @@
                 safetyandthreshold_go=false;
             }
             if (control_go==true) {
-                control();
+                float error1=0;
+                float error2=0;
+                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
                 control_go=false;
             }
             if (readsignal_go==true) {
@@ -247,20 +261,32 @@
             }
             valuechangebutton.fall(&valuechange);
         }
+        ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
         if (modecounter==1) {
             if(switchedmode==true) {
                 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
                 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;
+                mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
+                control_go=false;
+            }
+            ////////////////////////////////////////////CALIBRATE LEFT ARM
+            if (modecounter==2) {
+                if(switchedmode==true) {
+                    pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
+                    switchedmode=false;
+                }
+                if (control_go==true) {
+                    float error1=0;
+                    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 (modecounter==2) {
-            if(switchedmode==true) {
-                pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
-                switchedmode=false;
+                }
             }
-
         }
     }
 }
\ No newline at end of file