Diff: main.cpp

Revision:

15:6ad7abc5c691

Parent:

14:fb5d8064830d

Child:

16:438b330f5312

--- a/main.cpp	Thu Nov 01 19:34:05 2018 +0000
+++ b/main.cpp	Thu Nov 01 21:23:12 2018 +0000
@@ -20,14 +20,18 @@
 #include "QEI.h"
 //#include "HIDScope.h"
 
+//Clicker servo library
+#include "Servo.h"
+Servo myservo(A5);
+
 
 // GENERAL PIN DEFENITIONS
 MODSERIAL pc(USBTX, USBRX);
 
-// EMG     --      PIN DEFENITIONS 
+// EMG     --      PIN DEFENITIONS
 DigitalOut gpo(D0);
 
-DigitalIn button2(SW3);  
+DigitalIn button2(SW3);
 DigitalIn button1(SW2); //or SW2
 
 DigitalOut led1(LED_GREEN);
@@ -47,7 +51,7 @@
 AnalogIn emg4(A3); //left triceps
 
 
-// PID  CONTROLLER     --        PIN DEFENITIONS 
+// PID  CONTROLLER     --        PIN DEFENITIONS
 //AnalogIn button3(A4);
 //AnalogIn button4(A5);
 
@@ -186,7 +190,7 @@
     emg2_filtered = lowp2.step(emg2_abs);
     emg3_filtered = lowp3.step(emg3_abs);
     emg4_filtered = lowp4.step(emg4_abs);
-    
+
 
 
 }
@@ -297,6 +301,68 @@
 
 }
 
+//-----------------------------DEMO PART---------------------------------------
+
+// DEMO COORDINATES
+float px1 = 0.2;
+float py1 = 0.15;
+float px2 = 0.15;
+float py2 =0.15;
+float px3 = 0.25;
+float py3 = 0.15;
+float px5 = 0.2;
+float py5 = 0.2;
+float px6 = 0.2;
+float py6 = 0.1;
+
+void demomodus()
+{
+    if(t==2) {
+        px = px1;
+        py = py1;
+    } else if(t==4) {
+        px = px2;
+        py = py2;
+    } else if(t==6) {
+        px = px3;
+        py = py3;
+    } else if(t==8) {
+        px = px1;
+        py = py1;
+    } else if(t==10) {
+        px = px5;
+        py = py5;
+    } else if(t==12) {
+        px = px6;
+        py = py6;
+    } else if(t==14) {
+        px = px1;
+        py = py1;
+    } else if(t==16) {
+        px = px3;
+        py = py3;
+    } else if(t==18) {
+        px = px5;
+        py = py5;
+    } else if(t==20) {
+        px = px2;
+        py = py2;
+    } else if(t==22) {
+        px = px6;
+        py = py6;
+    } else if(t==24) {
+        px = px1;
+        py = py1;
+    }
+
+}
+
+
+
+//-----------------------------------------------------------------------------
+
+
+
 
 // ~~~~~~~~~~~~~~ROBOT KINEMATICS ~~~~~~~~~~~~~~~~~~
 
@@ -347,7 +413,7 @@
 
 //~~~~~~~~~~~~CALCULATIING MOTOR ANGLES ~~~~~~~~
 // arm 1 --> reference angle motor 1
-float hoek1(float px, float py) // input: ref x, ref y
+float hoek1(float px, float py)   // input: ref x, ref y
 {
     float c1x =  px - rp * cos(thetap +(M_PI/6));       // x locatie hoekpunt end-effector
     float c1y = py - rp*sin(thetap+(M_PI/6));           // y locatie hoekpunt end-effector
@@ -447,8 +513,8 @@
 void Motor_mover()
 {
     float px = positionx(bicepsR,bicepsL);  // EMG: +x, -x
-   float py = positiony(tricepsR,tricepsL);  // EMG: +y, -y
-        
+    float py = positiony(tricepsR,tricepsL);  // EMG: +y, -y
+
     double motor_position = encoder1.getPulses(); //output in counts
     double reference_rotation = hoek1(px, py);
     double error = reference_rotation - motor_position*(2*PI)/8400;
@@ -526,7 +592,7 @@
                 timer_calibration.reset();
                 timer_calibration.start();
 
-                sample_ticker.attach(&emgsample, ts); 
+                sample_ticker.attach(&emgsample, ts);
                 CalibrationEMG();
                 sample_ticker.detach();
                 timer_calibration.stop();
@@ -581,7 +647,12 @@
                 led2 = 1;
                 led3 = 0;
                 wait (1);
-
+                t.reset();
+                t.start();
+                demomodus();
+                    if(t>=26) {
+                    t.stop();
+                }
                 stateChanged = false;
             }
 
@@ -637,7 +708,7 @@
                 stateChanged = true;
             }
             // here ends the idle checking mode
-            else{
+            else {
                 //For every muscle a different colour if threshold is passed
                 if(bicepsR==1) {
                     led1 = 0;
@@ -688,7 +759,10 @@
                 led1 = 1;
                 led2 = 1;
                 led3 = 0;
-                wait (1);
+                for(float p=1; p>0; p -= 0.1) {
+                    myservo = p;
+                    wait(0.1);
+                }
 
                 stateChanged = false;
             }
@@ -707,7 +781,7 @@
 
 int main()
 {
-   
+
     //BiQuad Chain add
     highp1.add( &highp1_1 ).add( &highp1_2 );
     notch1.add( &notch1_1 ).add( &notch1_2 );
@@ -729,7 +803,7 @@
     led1 = 1;
     led2 = 1;
     led3 = 1;
-    
+
     pwmpin1.period_us(60); // setup motor
     ref_rot.attach(Motor_mover, 0.01f);// HAS TO GO TO STATE MACHINE
     //movement_ticker_activator();
@@ -737,40 +811,40 @@
     while (true) {
         ProcessStateMachine();
 
-/*      
-        if (button2 == false) {
-            wait(0.01f);
+        /*
+                if (button2 == false) {
+                    wait(0.01f);
 
-            // berekenen positie
-            float px = positionx(1,0);  // EMG: +x, -x
-            float py = positiony(0,0);  // EMG: +y, -y
-            //printf("positie (%f,%f)\n\r",px,py);
-        }
+                    // berekenen positie
+                    float px = positionx(1,0);  // EMG: +x, -x
+                    float py = positiony(0,0);  // EMG: +y, -y
+                    //printf("positie (%f,%f)\n\r",px,py);
+                }
 
-        if (button1 == false) {
-            wait(0.01f);
-            // berekenen positie
-            float px = positionx(0,1);  // EMG: +x, -x
-            float py = positiony(0,0);  // EMG: +y, -y
-            //printf("positie (%f,%f)\n\r",px,py);
-        }
-/*
-        if (button3 == false) {
-            wait(0.01f);
-            // berekenen positie
-            float px = positionx(0,0);  // EMG: +x, -x
-            float py = positiony(1,0);  // EMG: +y, -y
-            //printf("positie (%f,%f)\n\r",px,py);
-        }
+                if (button1 == false) {
+                    wait(0.01f);
+                    // berekenen positie
+                    float px = positionx(0,1);  // EMG: +x, -x
+                    float py = positiony(0,0);  // EMG: +y, -y
+                    //printf("positie (%f,%f)\n\r",px,py);
+                }
+        /*
+                if (button3 == false) {
+                    wait(0.01f);
+                    // berekenen positie
+                    float px = positionx(0,0);  // EMG: +x, -x
+                    float py = positiony(1,0);  // EMG: +y, -y
+                    //printf("positie (%f,%f)\n\r",px,py);
+                }
 
-        if (button4 == false) {
-            wait(0.01f);
-            // berekenen positie
-            float px = positionx(0,0);  // EMG: +x, -x
-            float py = positiony(0,1);  // EMG: +y, -y
-            //printf("positie (%f,%f)\n\r",px,py);
-        }
-*/
+                if (button4 == false) {
+                    wait(0.01f);
+                    // berekenen positie
+                    float px = positionx(0,0);  // EMG: +x, -x
+                    float py = positiony(0,1);  // EMG: +y, -y
+                    //printf("positie (%f,%f)\n\r",px,py);
+                }
+        */
     }
 
 }