Gaston Gabriël
met demo en click
Dependencies: biquadFilter MODSERIAL QEI Servo mbed
Fork of
StateMachine_EMg_RKI_PID_MOTORwerkend
by 
Casper Maas
Revision 15:6ad7abc5c691, committed 2018-11-01
- Comitter:
- gastongab
- Date:
- Thu Nov 01 21:23:12 2018 +0000
- Parent:
- 14:fb5d8064830d
- 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 |
diff -r fb5d8064830d -r 6ad7abc5c691 Servo.lib --- /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
diff -r fb5d8064830d -r 6ad7abc5c691 main.cpp
--- 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( ¬ch1_1 ).add( ¬ch1_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);
+ }
+ */
}
}