Thomas Plaisier / Mbed 2 deprecated G3_Barry_Plotter.

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:   MODSERIAL mbed Encoder

main.cpp@7:4d2edb5b0164, 2013-10-18 (annotated)

Committer:
Socrates
Date:
Fri Oct 18 10:05:56 2013 +0000
Revision:
7:4d2edb5b0164
Parent:
6:27a4e8d9ddac
Child:
8:43cce9f7a006
Met motor, doet het niet.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Socrates 0:b13a317308d1 1 #include "mbed.h"
Socrates 0:b13a317308d1 2 #include "MODSERIAL.h"
Socrates 5:1a1ce2f5cb66 3 AnalogIn emgt(PTB1);
Socrates 5:1a1ce2f5cb66 4 AnalogIn emgb(PTB0);
Socrates 0:b13a317308d1 5 MODSERIAL pc(USBTX,USBRX);
Socrates 7:4d2edb5b0164 6 PwmOut pwm_motor1(PTA12);
Socrates 7:4d2edb5b0164 7 PwmOut pwm_motor2(PTA5);
Socrates 0:b13a317308d1 8 volatile bool looptimerflag;
Socrates 0:b13a317308d1 9
Socrates 0:b13a317308d1 10 void setlooptimerflag(void)
Socrates 0:b13a317308d1 11 {
Socrates 0:b13a317308d1 12 looptimerflag = true;
Socrates 0:b13a317308d1 13 }
Socrates 0:b13a317308d1 14
Socrates 0:b13a317308d1 15
Socrates 0:b13a317308d1 16 int main()
Socrates 0:b13a317308d1 17 {
Socrates 0:b13a317308d1 18 Ticker looptimer;
Socrates 1:34202d107458 19 const float ts=0.001;
Socrates 1:34202d107458 20 looptimer.attach(setlooptimerflag,ts);
Socrates 5:1a1ce2f5cb66 21 float xt,yt,y1t,y2t,x1t,zt,z1t,z2t,yabst,yabs1t,yabs2t,kt,numh1,numh2,denh1,denh2,numl1,numl2,numl3,denl1,denl2,denl3;
Socrates 5:1a1ce2f5cb66 22 float xb,yb,y1b,y2b,x1b,zb,z1b,z2b,yabsb,yabs1b,yabs2b,kb;
Socrates 5:1a1ce2f5cb66 23 float z;
Socrates 5:1a1ce2f5cb66 24 x1t=0;
Socrates 5:1a1ce2f5cb66 25 y1t=0;
Socrates 5:1a1ce2f5cb66 26 y2t=0;
Socrates 5:1a1ce2f5cb66 27 z1t=0;
Socrates 5:1a1ce2f5cb66 28 z2t=0;
Socrates 5:1a1ce2f5cb66 29 yabs1t=0;
Socrates 5:1a1ce2f5cb66 30 yabs2t=0;
Socrates 5:1a1ce2f5cb66 31 x1b=0;
Socrates 5:1a1ce2f5cb66 32 y1b=0;
Socrates 5:1a1ce2f5cb66 33 y2b=0;
Socrates 5:1a1ce2f5cb66 34 z1b=0;
Socrates 5:1a1ce2f5cb66 35 z2b=0;
Socrates 5:1a1ce2f5cb66 36 yabs1b=0;
Socrates 5:1a1ce2f5cb66 37 yabs2b=0;
Socrates 2:3dab90d3aac2 38
Socrates 6:27a4e8d9ddac 39 //High pass, 35Hz, 1e
Socrates 6:27a4e8d9ddac 40 numh1=0.900575535279376;
Socrates 6:27a4e8d9ddac 41 numh2=-0.900575535279376;
Socrates 2:3dab90d3aac2 42 //denh1=1;
Socrates 6:27a4e8d9ddac 43 denh2=-0.801151070558751;
Socrates 6:27a4e8d9ddac 44
Socrates 6:27a4e8d9ddac 45 //Low pass, 2 Hz, 2e orde
Socrates 4:513c33bba011 46 numl1=0.391302053991682*pow(10.0,-4.0);
Socrates 4:513c33bba011 47 numl2=0.782604107983365*pow(10.0,-4.0);
Socrates 4:513c33bba011 48 numl3=0.391302053991682*pow(10.0,-4.0);
Socrates 2:3dab90d3aac2 49 //denl1=1;
Socrates 4:513c33bba011 50 denl2=-1.982228929792529;
Socrates 4:513c33bba011 51 denl3=0.982385450614126;
Socrates 0:b13a317308d1 52 pc.baud(115200);
Socrates 0:b13a317308d1 53
Socrates 0:b13a317308d1 54 while(1) {
Socrates 0:b13a317308d1 55 while(looptimerflag != true);
Socrates 0:b13a317308d1 56 looptimerflag = false;
Socrates 6:27a4e8d9ddac 57
Socrates 5:1a1ce2f5cb66 58 //Triceps
Socrates 5:1a1ce2f5cb66 59 kt=emgt.read();
Socrates 5:1a1ce2f5cb66 60 xt=(kt-0.5)*2.0;
Socrates 2:3dab90d3aac2 61
Socrates 6:27a4e8d9ddac 62 //High pass
Socrates 4:513c33bba011 63 //1e orde
Socrates 5:1a1ce2f5cb66 64 yt=xt*numh1+x1t*numh2-y1t*denh2;
Socrates 6:27a4e8d9ddac 65
Socrates 1:34202d107458 66 //Rectify
Socrates 5:1a1ce2f5cb66 67 yabst=abs(yt);
Socrates 1:34202d107458 68
Socrates 6:27a4e8d9ddac 69 //Low pass
Socrates 4:513c33bba011 70 //2e orde
Socrates 5:1a1ce2f5cb66 71 zt=yabst*numl1+yabs1t*numl2+yabs2t*numl3-z1t*denl2-z2t*denl3;
Socrates 6:27a4e8d9ddac 72
Socrates 5:1a1ce2f5cb66 73 x1t=xt;
Socrates 5:1a1ce2f5cb66 74 y2t=y1t;
Socrates 5:1a1ce2f5cb66 75 y1t=yt;
Socrates 5:1a1ce2f5cb66 76 z2t=z1t;
Socrates 5:1a1ce2f5cb66 77 z1t=zt;
Socrates 5:1a1ce2f5cb66 78 yabs2t=yabs1t;
Socrates 5:1a1ce2f5cb66 79 yabs1t=yabst;
Socrates 6:27a4e8d9ddac 80
Socrates 5:1a1ce2f5cb66 81 //Biceps
Socrates 5:1a1ce2f5cb66 82 kb=emgb.read();
Socrates 5:1a1ce2f5cb66 83 xb=(kb-0.5)*2.0;
Socrates 5:1a1ce2f5cb66 84
Socrates 6:27a4e8d9ddac 85 //High pass
Socrates 5:1a1ce2f5cb66 86 //1e orde
Socrates 5:1a1ce2f5cb66 87 yb=xb*numh1+x1b*numh2-y1b*denh2;
Socrates 6:27a4e8d9ddac 88
Socrates 5:1a1ce2f5cb66 89 //Rectify
Socrates 5:1a1ce2f5cb66 90 yabsb=abs(yb);
Socrates 5:1a1ce2f5cb66 91
Socrates 6:27a4e8d9ddac 92 //Low pass
Socrates 5:1a1ce2f5cb66 93 //2e orde
Socrates 5:1a1ce2f5cb66 94 zb=yabsb*numl1+yabs1b*numl2+yabs2b*numl3-z1b*denl2-z2b*denl3;
Socrates 6:27a4e8d9ddac 95
Socrates 5:1a1ce2f5cb66 96 x1b=xb;
Socrates 5:1a1ce2f5cb66 97 y2b=y1b;
Socrates 5:1a1ce2f5cb66 98 y1b=yb;
Socrates 5:1a1ce2f5cb66 99 z2b=z1b;
Socrates 5:1a1ce2f5cb66 100 z1b=zb;
Socrates 5:1a1ce2f5cb66 101 yabs2b=yabs1b;
Socrates 5:1a1ce2f5cb66 102 yabs1b=yabsb;
Socrates 6:27a4e8d9ddac 103
Socrates 6:27a4e8d9ddac 104 if (zb>zt) {
Socrates 6:27a4e8d9ddac 105 z=zb;
Socrates 6:27a4e8d9ddac 106 } else {
Socrates 6:27a4e8d9ddac 107 z=zt;
Socrates 5:1a1ce2f5cb66 108 }
Socrates 6:27a4e8d9ddac 109
Socrates 6:27a4e8d9ddac 110 if (z>1.0) {
Socrates 6:27a4e8d9ddac 111 z=1.0;
Socrates 5:1a1ce2f5cb66 112 }
Socrates 7:4d2edb5b0164 113 pwm_motor1=z;
Socrates 7:4d2edb5b0164 114 pwm_motor2=z;
Socrates 7:4d2edb5b0164 115 pc.printf("%f\n\r",z);
Socrates 0:b13a317308d1 116 }
Socrates 0:b13a317308d1 117 }