Diff: main.cpp

Revision:

14:4c4f45a1dd23

Parent:

13:47b065aadae9

Child:

15:17de575b7385

--- a/main.cpp	Mon Oct 19 08:43:26 2015 +0000
+++ b/main.cpp	Mon Oct 19 09:13:01 2015 +0000
@@ -10,6 +10,10 @@
 const double scope_frequency=500;
 Serial pc(USBTX,USBRX);
 
+DigitalOut ledred(LED1);
+DigitalOut ledgreen(LED2);
+DigitalOut ledblue(LED3);
+
 ////////////////ENCODERS
 const float cpr_sensor=32;
 const float cpr_shaft=cpr_sensor*131;
@@ -25,11 +29,16 @@
 Ticker readbuttoncalibrate_ticker;
 const double readbuttoncalibrate_frequency=10;
 
+Ticker ledblink_ticker;
+const double ledblink_frequency=4;
+
 const double radpersec_calibrate=0.1*PIE;
 
 DigitalIn buttonR(D2);
 DigitalIn buttonL(D3);
 
+
+
 //////////////////////////////////CONTROLLER
 controlandadjust mycontroller; // make a controller
 //controller constants
@@ -93,7 +102,8 @@
               safetyandthreshold_go=false,
               readsignal_go=false,
               switchedmode=true,
-              readbuttoncalibrate_go=false;
+              readbuttoncalibrate_go=false,
+              ledblink_go=false;
 
 void scopedata_activate()
 {
@@ -119,6 +129,10 @@
 {
     readbuttoncalibrate_go=true;
 }
+void ledblink_activate()
+{
+    ledblink_go=true;
+}
 
 ////////////////////////FUNCTIONS
 //gather data and send to scope
@@ -206,7 +220,7 @@
 const float schiethoek=0.35*PIE;
 const float schiettijd=0.5;
 void shoot() // THIS NEEDS ADJUSTMEND
-{    
+{
     pc.printf("SHOOT\n");
     //hoeken groter maken
     desired_angle[0]-=schiethoek;
@@ -218,15 +232,16 @@
     float pass=0;
     while(schiettimer.read()<=schiettijd) {
 // errors berekenen en naar de controller passen
-       float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
-       float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
+        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);
         scopedata();
         wait (Ts_control-(schiettimer.read()-Ts_control*pass)); // even wachten anders wordt de while loop te snel doorlopen en gaan de motoren wak
+        ledblue=!ledblue;
         pass++;
     }
     schiettimer.stop();
-
+    ledblue=0;
     //terug na schieten
     desired_angle[0]+=schiethoek;
     desired_angle[1]-=schiethoek;
@@ -238,7 +253,7 @@
     if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
         led1=led2=1;
         shoot();
-    // check if pod has to move to the right
+        // check if pod has to move to the right
     } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
         led1=1;
         led2=0;
@@ -248,13 +263,13 @@
         } else {
             desired_position=desired_position;
         }
-    // check if pod has to move to the left
+        // check if pod has to move to the left
     } 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
         if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
-        desired_position=(-1*maxdisplacement);
+            desired_position=(-1*maxdisplacement);
         } else {
             desired_position=desired_position;
         }
@@ -289,6 +304,11 @@
     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);
+    ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
+
+    ledred=0;
+    ledgreen=0;
+    ledblue=0;
 
     while(1) {
         if (changemodebutton==0) {
@@ -308,6 +328,8 @@
                 encoder1.reset();
                 encoder2.reset();
                 pc.printf("Program running\n");//
+                ledgreen=1;
+                led1=led2=ledred=0;
                 switchedmode=false;
             }
 
@@ -332,6 +354,11 @@
             if(switchedmode==true) {
                 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
                 switchedmode=false;
+                ledred=1;
+                led1=led2=ledgreen=0;
+            }
+            if (ledblink_go==true) {
+                led1=!led1;
             }
             if (control_go==true) {
                 float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
@@ -354,8 +381,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;
                 switchedmode=false;
             }
+            if (ledblink_go==true) {
+                led2=!led2;
+            }
             if (control_go==true) {
                 float error1=0;
                 float error2=(desired_angle[1]-counttorad*encoder2.getPulses());// this is the error you want to use