Thomas Plaisier
/
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
Revision 22:a658a3e7b3f4, committed 2013-10-29
- Comitter:
- Socrates
- Date:
- Tue Oct 29 13:44:03 2013 +0000
- Parent:
- 21:659f7c8ed328
- Child:
- 23:ee89be59ae2f
- Commit message:
- Doet niet.
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 29 12:17:41 2013 +0000
+++ b/main.cpp Tue Oct 29 13:44:03 2013 +0000
@@ -3,8 +3,8 @@
#include "encoder.h"
#define PI 3.14159265358979323
//XenY
-//Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks.
-//4123 ticks is een rondje.
+//Een pwm van 0.05 is net genoeg om de heugel te bewegen. linksom bewegen is negatief voor de encoder. getposition gaat in ticks.
+//4123 ticks is een rondje.
//Rechts is x, links is y
AnalogIn emgtr(PTB3);
AnalogIn emgbr(PTB2);
@@ -23,7 +23,7 @@
volatile bool looptimerflag;
void setlooptimerflag(void)
{
- looptimerflag = true;
+looptimerflag = true;
}
volatile bool dirflagx=true;
@@ -31,8 +31,8 @@
void tricheck(void)
{
- dirflagx=true;
- dirflagy=true;
+dirflagx=true;
+dirflagy=true;
}
int main()
@@ -49,61 +49,28 @@
float xtl,ytl,y1tl,x1tl,ztl,z1tl,z2tl,yabstl,yabs1tl,yabs2tl,ktl;
float xbl,ybl,y1bl,x1bl,zbl,z1bl,z2bl,yabsbl,yabs1bl,yabs2bl,kbl;
float zx,zy;
- float xuit,yuit, rt;
- //float zx1, zy1, gain;
- //float pwmA, pwmB;
+ float xuit,yuit, rt;
int xdir, ydir, Adir, Bdir;
float vxuit, vyuit, Ap, Ad, Ad1, kd, kp, ki, Ai, Ai1, Bp, Bd, Bd1, Bi, Bi1;
- float for_A, for_B, yuit1, xuit1;
+ float for_A, for_B;
int ticka, tickb, refA,refB, errA, errB;
float ctrlA;
float ctrlB;
//Startwaarden
- x1tr=0;
- y1tr=0;
- z1tr=0;
- z2tr=0;
- yabs1tr=0;
- yabs2tr=0;
- x1br=0;
- y1br=0;
- z1br=0;
- z2br=0;
- yabs1br=0;
- yabs2br=0;
- x1tl=0;
- y1tl=0;
- z1tl=0;
- z2tl=0;
- yabs1tl=0;
- yabs2tl=0;
- x1bl=0;
- y1bl=0;
- z1bl=0;
- z2bl=0;
- yabs1bl=0;
- yabs2bl=0;
- zx=0;
- zy=0;
- xdir=0;
- ydir=0;
- xuit=0;
- yuit=0;
- kp=0.1;
- ki=0.01;
- kd=0.01;
- rt=0.032805;
- Bi1=0;
- Bd1=0;
- yuit1=0;
- xuit1=0;
+ x1tr=0; y1tr=0; z1tr=0; z2tr=0; yabs1tr=0; yabs2tr=0;
+ x1br=0; y1br=0; z1br=0; z2br=0; yabs1br=0; yabs2br=0;
+ x1tl=0; y1tl=0; z1tl=0; z2tl=0; yabs1tl=0; yabs2tl=0;
+ x1bl=0; y1bl=0; z1bl=0; z2bl=0; yabs1bl=0; yabs2bl=0;
+ zx=0; zy=0; xdir=0; ydir=0; xuit=0; yuit=0;
+ kp=1; ki=0.0; kd=0.0; rt=0.032805; Ai1=0; Ad1=0;
+ Bi1=0; Bd1=0;
//High pass, 35Hz, 1e orde
numh1=0.900575535279376;
numh2=-0.900575535279376;
//denh1=1;
denh2=-0.801151070558751;
-
+
//Low pass, 5 Hz, 2e orde.
numl1=0.241359049041961*pow(10.0,-3.0);
numl2=0.482718098083923*pow(10.0,-3.0);
@@ -114,63 +81,42 @@
pc.baud(115200);
while(1) {
- while(looptimerflag != true);
- {
+ while(looptimerflag != true);{
}
looptimerflag = false;
//EMG lezen.
- ktr=emgtr.read();
+ktr=emgtr.read();
xtr=(ktr-0.5)*2.0;
ytr=xtr*numh1+x1tr*numh2-y1tr*denh2;
yabstr=abs(ytr);
ztr=yabstr*numl1+yabs1tr*numl2+yabs2tr*numl3-z1tr*denl2-z2tr*denl3;
- x1tr=xtr;
- y1tr=ytr;
- z2tr=z1tr;
- z1tr=ztr;
- yabs2tr=yabs1tr;
- yabs1tr=yabstr;
+ x1tr=xtr; y1tr=ytr; z2tr=z1tr; z1tr=ztr; yabs2tr=yabs1tr; yabs1tr=yabstr;
- kbr=emgbr.read();
+kbr=emgbr.read();
xbr=(kbr-0.5)*2.0;
ybr=xbr*numh1+x1br*numh2-y1br*denh2;
yabsbr=abs(ybr);
zbr=yabsbr*numl1+yabs1br*numl2+yabs2br*numl3-z1br*denl2-z2br*denl3;
- x1br=xbr;
- y1br=ybr;
- z2br=z1br;
- z1br=zbr;
- yabs2br=yabs1br;
- yabs1br=yabsbr;
+ x1br=xbr; y1br=ybr; z2br=z1br; z1br=zbr; yabs2br=yabs1br; yabs1br=yabsbr;
- ktl=emgtl.read();
+ktl=emgtl.read();
xtl=(ktl-0.5)*2.0;
ytl=xtl*numh1+x1tl*numh2-y1tl*denh2;
yabstl=abs(ytl);
ztl=yabstl*numl1+yabs1tl*numl2+yabs2tl*numl3-z1tl*denl2-z2tl*denl3;
- x1tl=xtl;
- y1tl=ytl;
- z2tl=z1tl;
- z1tl=ztl;
- yabs2tl=yabs1tl;
- yabs1tl=yabstl;
+ x1tl=xtl; y1tl=ytl; z2tl=z1tl; z1tl=ztl; yabs2tl=yabs1tl; yabs1tl=yabstl;
- kbl=emgbl.read();
+kbl=emgbl.read();
xbl=(kbl-0.5)*2.0;
ybl=xbl*numh1+x1bl*numh2-y1bl*denh2;
yabsbl=abs(ybl);
zbl=yabsbl*numl1+yabs1bl*numl2+yabs2bl*numl3-z1bl*denl2-z2bl*denl3;
- x1bl=xbl;
- y1bl=ybl;
- z2bl=z1bl;
- z1bl=zbl;
- yabs2bl=yabs1bl;
- yabs1bl=yabsbl;
+ x1bl=xbl; y1bl=ybl; z2bl=z1bl; z1bl=zbl; yabs2bl=yabs1bl; yabs1bl=yabsbl;
- zx=(zbr*5.0);
- zy=(zbl*5.0);
-
+ zx=(zbr*3.0);
+ zy=(zbl*3.0);
+
//Grenzen.
if (zx>1.0) {
zx=0.99999;
@@ -178,94 +124,91 @@
if (zy>1.0) {
zy=0.99999;
}
- if (zx<0.20) {
- zx=0;
+ if (zx<0.30){
+ zx=0;
}
- if (zy<0.20) {
- zy=0;
+ if (zy<0.30){
+ zy=0;
}
-
- //Richting omdraaien met triceps.
+
+ //Richting omdraaien met triceps.
if ((ztr>(zbr+0.1)) && dirflagx == true) {
dirflagx = false;
xdir ^= 1;
zx=0;
dirtimeout.attach(tricheck,1.5);
}
- if ((ztl>(zbl+0.1)) && dirflagy == true) {
+ if ((ztl>(zbl+0.1)) && dirflagy == true) {
dirflagy = false;
ydir ^= 1;
zy=0;
dirtimeout.attach(tricheck,1.5);
}
-
+
//Motoraansturing.
- if (ydir==1) {
- zy=-1.0*zy;
+ if (ydir==1)
+ {
+ zy=-1.0*zy;
}
- if (xdir==1) {
- zx=-1.0*zx;
+ if (xdir==1)
+ {
+ zx=-1.0*zx;
}
-
- ticka=motor1.getPosition();
- tickb=motor2.getPosition();
- vxuit=zx*1.0*pow(1.0,-2.0); // 4cm/s
- vyuit=zy*1.0*pow(1.0,-2.0); // 4cm/s
+
+ ticka=motor1.getPosition(); tickb=motor2.getPosition();
+ vxuit=zx*1.0*pow(1.0,1.0); // 4cm/s
+ vyuit=zy*1.0*pow(1.0,1.0); // 4cm/s
xuit += ts*vxuit;
yuit += ts*vyuit;
- yuit1=yuit;
-
+
refA=floor(4123.0*atan2(yuit,xuit)/(2.0*PI));
refB=floor(4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt));
errA=refA-ticka;
- errB=refB-tickb;
-
- if(errA<0) {
- Adir=0;
- } else {
- Adir=1;
- }
- if(errB<0) {
- Bdir=0;
- } else {
- Bdir=1;
- }
+ errB=refB-tickb;
+
+if(errA<0){
+Adir=0;}
+else{
+Adir=1;
+}
+if(errB<0){
+Bdir=0;}
+else{
+Bdir=1;
+}
//Controllers
- Ap=errA*kp;
- Ad=(errA-Ad1)*kd/ts;
- Ai=(Ai1+ts*errA)*ki;
- keep_in_range(&Ai, -0.1,0.1);
- Ad1=Ad;
- Ai1=Ai;
- ctrlA=Ai+Ap+Ad;
- for_A = ctrlA/1000.;
+ Ap=errA*kp; Ad=(errA-Ad1)*kd/ts; Ai=(Ai1+ts*errA)*ki;
+ keep_in_range(&Ai,-0.1,0.1);
+ keep_in_range(&Ad,-0.1,0.1);
+ Ad1=Ad; Ai1=Ai;
+ ctrlA=(Ai+Ap+Ad);
+ for_A=(ctrlA)/1000.0;
- errB= 1;
- Bp=errB*kp;
- Bd=(errB-Bd1)*kd/ts;
- Bi=(Bi1+ts*errB)*ki;
- keep_in_range(&Bi, -0.1,0.1);
- Bd1=Bd;
- Bi1=Bi;
- ctrlB=Bi+Bp+Bd;
- for_B= ctrlB / 1000.0;
- //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
-
+ Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; Bi=(Bi1+ts*errB)*ki;
+ keep_in_range(&Bi,-0.1,0.1);
+ keep_in_range(&Bd,-0.1,0.1);
+ Bd1=Bd; Bi1=Bi;
+ ctrlB=(Bi+Bp+Bd);
+ for_B=(ctrlB)/1000.0;
+ //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
+
keep_in_range(&for_A, -1,1);
keep_in_range(&for_B, -1,1);
motordirA.write(Adir);
motordirB.write(Bdir);
- pwm_A.write(for_A);
- pwm_B.write(for_B);
-
- //pc.printf("Bi: %f, Tri: %f, Richting: %d \n\r",zbl*3.0,ztl*3.0,ydir);
- pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
- //pc.printf("b %f t %f xu %f yu %f A %f B %f \n\r",zbl,ztl,xuit, yuit, pwmA,pwmB);
+ pwm_A.write(abs(for_A));
+ pwm_B.write(abs(for_B));
+
+ //Voor EMG
+ pc.printf(" %f \n\r", zy);
+
+ //Voor motor
+ //pc.printf(" %i %i %i %f %f\n\r",tickb,refB,errB,ctrlB,for_B);
}
}
void keep_in_range(float * in, float min, float max)
{
-*in > min ? *in < max? : *in = max: *in = min;
+ *in > min ? *in < max? : *in = max: *in = min;
}
\ No newline at end of file
