Diff: main.cpp

Revision:

27:0dbd4fd88593

Parent:

26:c9ba45bdd5c9

Child:

28:fc099a3e37fd

--- a/main.cpp	Thu Nov 03 15:20:48 2016 +0000
+++ b/main.cpp	Fri Nov 04 04:32:35 2016 +0000
@@ -3,6 +3,20 @@
 #include "BiQuad.h"
 #include "MODSERIAL.h"
 
+#include "FastPWM.h"  
+#include "QEI.h"
+float pwmprocent1 = 15;             // introduce duty cycle variable translational motor
+float pwmprocent2 = 15;             // introduce duty cycle for rotational motor (SLOW DOWN TO PREVENT OVERSHOOT?)
+int modesw;                 // introduce mode to switch from translational to rotational control
+FastPWM motorpwm1(D5);       // define PWM Pin
+FastPWM motorpwm2(D6);      // define motor 2 pwm pin
+float totalpwm1 = pwmprocent1/100;
+float totalpwm2 = pwmprocent2/100;
+int upperlimit = 1200; 
+int lowerlimit = 3920;
+DigitalOut m1direction(D4);     // direction translational motor
+DigitalOut m2direction(D7);     // direction
+
 //Define objects
     AnalogIn    emg1_in( A0 ); /* read out the signal */
     AnalogIn    emg2_in( A1 );
@@ -66,6 +80,41 @@
     BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
     //
 
+// stopfunction
+void superstop()
+{
+    int stoptimes;
+    if (modesw==0)
+    {
+         motorpwm1.write(0);
+         for (int a = 0; a < pwmprocent1; a = a + 1)
+         {
+             if (m1direction = 0)
+             {
+                 m1direction = 1;
+             }
+             else
+             {
+                 m1direction = 0;
+             }
+         }
+     else
+     {
+         motorpwm2.write(0);
+         for (int a = 0; a < pwmprocent2; a = a+1)
+         {
+             if (m2direction = 0)
+             {
+                 m2direction = 1;
+             }
+             else
+             {
+                 m2direction = 0;
+             }
+         }
+     } 
+}
+
 // Finding max values for correct motor switch if the button is pressed
 void get_max1(){
     if (max_reader1==0){
@@ -141,19 +190,52 @@
     emg3abs = fabs(emg3notchfilter);
     emg3lowfilter = filterlow3.step(emg3abs);
     emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
-    
-    
+    int count;
+    QEI Encoder(D12,D13,NC,64);
     /* Compare measurement to the calibrated value to decide actions */
     if (maxpart1<emg1peak){ /* See if right biceps is contracting */
         red = 0;
         blue = 1;
         green = 1;
+        
+        switch (modesw)
+        {
+            case 0:
+            {
+                modesw = 1;
+                break;
+            }
+            case 1:
+            {
+                modesw = 0;
+                break;
+            }
+
         }
     else {
         if (maxpart2<emg2peak){ /* See if left biceps is contracting */
             red = 1;
             blue = 0;
             green = 1;
+   
+                switch (modesw)
+                {
+                    case 0:
+                    {
+                        m1direction = 1;
+                        motorpwm1.write(totalpwm1);
+                        break;
+                    }
+                    case 1:
+                    {
+                        count = Encoder.getPulses();
+                        m2direction = 1;
+                        if (count<lowerlimit
+                            motorpwm2.write(totalpwm2);
+                        break;
+                    }
+                }
+                
             }
         
     else {
@@ -161,6 +243,21 @@
             red = 1;
             blue = 1;
             green = 0;
+                        
+            switch (modesw)
+            {
+                case 0:
+                {
+                    m1direction = 0;
+                    motorpwm1.write(totalpwm1);
+                    break;
+                }
+                case 1:
+                {             
+                    m1direction = 0;
+                    motorpwm2.write(totalpwm1);
+                    break;
+                }
             }
         
     else {