Diff: main.cpp

Revision:

33:1c7b498ded25

Parent:

32:c940f6b6a6a0

Child:

34:bc2d64e86cb9

--- a/main.cpp	Wed Oct 28 13:09:25 2015 +0000
+++ b/main.cpp	Wed Oct 28 14:58:35 2015 +0000
@@ -96,7 +96,7 @@
 
 volatile double filteredsignal1=0;//the first filtered emg signal
 volatile double filteredsignal2=0;//the second filtered emg signal
-float filter_extragain=1;
+float filter_extragain1=1,filter_extragain2=1;
 
 
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
@@ -163,11 +163,11 @@
 //gather data and send to scope
 void scopedata(double wanted_y)
 {
-    scope.set(0,desired_position-anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()));
-    scope.set(1,wanted_y-anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()));
-    scope.set(2,error1*radtodeg);
-    scope.set(3,error2*radtodeg);
-    scope.set(4,mycontroller.motor1pwm());
+    scope.set(0,desired_position);
+    scope.set(1,wanted_y);
+    scope.set(2,filteredsignal1);
+    scope.set(3,filteredsignal2);
+    scope.set(4,threshold_value);
     scope.set(5,mycontroller.motor2pwm());
     scope.send();
 }
@@ -213,8 +213,8 @@
     double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
     double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
 
-    filteredsignal1=(pass7_emg1*9e11*filter_extragain);
-    filteredsignal2=(pass7_emg2*9e11*filter_extragain);
+    filteredsignal1=(pass7_emg1*9e11*filter_extragain1);
+    filteredsignal2=(pass7_emg2*9e11*filter_extragain2);
 
     if (makeempty==true) {//this is needed so the filtered value is nog high after shooting
         pass1_emg1 = pass1_emg2 =pass2_emg1 =pass2_emg2 =pass3_emg1 = pass3_emg2 =pass4_emg1 =pass4_emg2 =pass5_emg1 =pass5_emg2 =0;
@@ -228,20 +228,11 @@
 void valuechange()
 {
     mycontroller.STOP();
-    pc.printf("KP shoot is now %f, enter new value\n",Kp_shoot);
-    pc.scanf("%f", &Kp_shoot);
-
-    pc.printf("KI shoot is now %f, enter new value\n",Ki_shoot);
-    pc.scanf("%f", &Ki_shoot);
-
-    pc.printf("KD shoot is now %f, enter new value\n",Kd_shoot);
-    pc.scanf("%f", &Kd_shoot);
-
-    pc.printf("Factor of tau_p=1.0/(factor*controlfrequency) is now %f, enter new value\n",factor_taup);
-    pc.scanf("%f", &factor_taup);
-
-    pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
-    pc.scanf("%f", &filter_extragain);
+       pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1);
+    pc.scanf("%f", &filter_extragain1);
+    
+    pc.printf("Extra gain for left arm is now %f, enter new value\n",filter_extragain2);
+    pc.scanf("%f", &filter_extragain2);
 }
 
 // shoot the pod forward
@@ -261,14 +252,14 @@
         ledblue=!ledblue;
 
         profile_angle+=stepsize; // add stepsize to angle
-        
+
         y_during_punch=(y_punch-y_start)*(1-cos(profile_angle))+y_start; // calculate y position for shooting
-        
+
         if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety
             y_during_punch=y_punch;
         } else {
             y_during_punch=y_during_punch;
-        }        
+        }
 
         desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles
         desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch);
@@ -293,7 +284,7 @@
         ledblue=!ledblue;
 
         profile_angle+=stepsize; // add stepsize to y position
-       y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting
+        y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting
         if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety
             y_during_punch=y_start;
         } else {
@@ -391,7 +382,7 @@
     mycontroller.STOP();
     switchedmode=true;
     modecounter++;
-    if (modecounter==4) {
+    if (modecounter==5) {
         modecounter=0;
     } else {
         modecounter=modecounter;
@@ -462,7 +453,7 @@
                 mycontroller.PID(error1,error2,Kp_move,Ki_move,Kd_move);
                 control_go=false;
             }
-            valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
+           // valuechangebutton.fall(&valuechange);// used to change controller variables and the gain for the filter
         }
         ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
         if (modecounter==1) {
@@ -550,6 +541,24 @@
                 control_go=false;
             }
         }
+        if(modecounter==4) {
+            if (switchedmode==true) {
+                pc.printf("Calibrate the EMG signals and threshold");
+                ledgreen=1;
+                ledred=0;
+                switchedmode=false;
+            }
+            if(ledblink_go==true) {
+                ledgreen=!ledgreen;
+                ledred=!ledred;
+                ledblink_go=false;
+            }
+             if (filter_go==true) {// filter the emg signal
+                // TIME THIS LOOP TAKES: 0.000173 SEC
+                filtereverything(false);
+                filter_go=false;
+            }
+        }
         checktimervalue=checktimer.read();
         checktimer.stop();
     }