Diff: main.cpp

Revision:

8:54a7da09ccad

Parent:

7:7fbb2c028778

Child:

9:4ee354663560

--- a/main.cpp	Wed Oct 14 13:42:28 2015 +0000
+++ b/main.cpp	Wed Oct 14 14:17:14 2015 +0000
@@ -18,20 +18,25 @@
 const float counttorad=(cpr/(2*PIE));
 
 /////////////////////////////////CALIBRATION (MODE)
-int modecounter=0;
+int modecounter=1;
 DigitalIn changemodebutton(PTA4);
 
 Ticker readbuttoncalibrate_ticker;
 const double readbuttoncalibrate_frequency=10;
 
+const double radpersec_calibrate=0.1;
+
+DigitalIn buttonR(D2);
+DigitalIn buttonL(D3);
+
 //////////////////////////////////CONTROLLER
 controlandadjust mycontroller; // make a controller
 //controller constants
 float Kp=0.5;
-float Ki=0.01;
+float Ki=0.001;
 float Kd=0.001;
 Ticker control_ticker;
-const double control_frequency=25;
+const double control_frequency=50;
 const double Ts_control=1.0/control_frequency;
 
 float error1_int=0;// storage variables for the errors
@@ -81,7 +86,7 @@
               filter_go=false,
               safetyandthreshold_go=false,
               readsignal_go=false,
-              switchedmode=false,
+              switchedmode=true,
               readbuttoncalibrate_go=false;
 
 void scopedata_activate()
@@ -113,8 +118,9 @@
 //gather data and send to scope
 void scopedata()
 {
-    scope.set(0,emg1_input.read());
-    scope.set(1,filteredsignal1);
+    scope.set(0,desired_angle[1]);
+    scope.set(1,counttorad*encoder2.getPulses());
+    scope.set(2,mycontroller.motor2pwm());
     scope.send();
 }
 //read potmeters and adjust the safetyfactor and threshold
@@ -273,20 +279,38 @@
                 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;
+        }
+        ////////////////////////////////////////////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 (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) {
+                    desired_angle[1] += (radpersec_calibrate/readbuttoncalibrate_frequency);
+                    readbuttoncalibrate_go=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 (buttonR.read()==1 && buttonL.read()==0) {
+                    desired_angle[1] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
+                    readbuttoncalibrate_go=false;
                 }
             }
         }
     }
-}
\ No newline at end of file
+}