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
Diff: main.cpp
- Revision:
- 21:659f7c8ed328
- Parent:
- 20:013e9c00e058
- Child:
- 22:a658a3e7b3f4
diff -r 013e9c00e058 -r 659f7c8ed328 main.cpp
--- a/main.cpp Tue Oct 29 11:11:19 2013 +0000
+++ b/main.cpp Tue Oct 29 12:17:41 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,7 +49,7 @@
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 xuit,yuit, rt;
//float zx1, zy1, gain;
//float pwmA, pwmB;
int xdir, ydir, Adir, Bdir;
@@ -59,20 +59,51 @@
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.01; 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=0.1;
+ ki=0.01;
+ kd=0.01;
+ rt=0.032805;
+ Bi1=0;
+ Bd1=0;
+ yuit1=0;
+ xuit1=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);
@@ -83,42 +114,63 @@
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);
-
+
//Grenzen.
if (zx>1.0) {
zx=0.99999;
@@ -126,71 +178,79 @@
if (zy>1.0) {
zy=0.99999;
}
- if (zx<0.20){
- zx=0;
+ if (zx<0.20) {
+ zx=0;
}
- if (zy<0.20){
- zy=0;
+ if (zy<0.20) {
+ 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();
+
+ 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
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;
+ 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.;
-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;
- Ad1=Ad; Ai1=Ai;
- ctrlA=float(Ai+Ap+Ad);
- for_A=(ctrlA)/1000.0;
-
- Bp=errB*kp; Bd=(errB-Bd1)*kd/ts; Bi=(Bi1+ts*errB)*ki;
- Bd1=Bd; Bi1=Bi;
- ctrlB=float(Bi+Bp+Bd);
- for_B=(ctrlB)/1000.0;
- //x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
-
+ 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.
+
keep_in_range(&for_A, -1,1);
keep_in_range(&for_B, -1,1);
@@ -198,7 +258,7 @@
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);
@@ -207,5 +267,5 @@
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
