Astrid Schut / Mbed 2 deprecated Robot_Battle_met_ARVID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   biquadFilter MODSERIAL QEI Servo mbed

Fork of Robot_Battle_met_ARVID by Gaston Gabriël

Files at this revision

API Documentation at this revision

Revision 15:6ad7abc5c691, committed 2018-11-01

Comitter:
gastongab
Date:
Thu Nov 01 21:23:12 2018 +0000
Parent:
14:fb5d8064830d
Child:
16:438b330f5312
Commit message:
met clicker en demo

Changed in this revision

Servo.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Thu Nov 01 21:23:12 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- 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);
+                }
+        */
     }
 
 }