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:
- 33:c495e9d8ea1f
- Parent:
- 32:5ae627e1bce8
- Child:
- 34:961aec0a13c6
--- a/main.cpp Mon Nov 04 09:27:22 2013 +0000
+++ b/main.cpp Mon Nov 04 15:27:44 2013 +0000
@@ -14,6 +14,9 @@
AnalogIn emgbl(PTB0);
PwmOut pwm_A(PTA12);
PwmOut pwm_B(PTA5);
+PwmOut redled(LED_RED);
+PwmOut greenled(LED_GREEN);
+PwmOut blueled(LED_BLUE);
MODSERIAL pc(USBTX,USBRX);
DigitalOut motordirA(PTD3);
DigitalOut motordirB(PTD1);
@@ -102,7 +105,7 @@
kic=0.1*1.0;
kp=1*0.1545;
kd=1.0*2.8*pow(10.0,-3.0);
- ki=1.0*1.0;
+ ki=0.1*1.0;
rt=0.032805;
gain=4.0;
emggrens=0.35;
@@ -144,7 +147,7 @@
if(startflag==true);
{
- motor1.setPosition(0);
+ motor1.setPosition(200);
motor2.setPosition(1200);
startflag=false;
}
@@ -177,15 +180,14 @@
}
pwm_B.write(abs(for_B));
motordirB.write(Bdir);
- //pc.printf("B %f %i \n\r",for_B,errB);
- if(errB<10) {
+ if(errB<40) {
calB=false;
pwm_B.write(0.0);
}
}
while(calA==true) {
- ticka=-1*motor1.getPosition(); //NOTE: deze moet volgens mij gewoon weer positief zijn.
+ ticka=-1*motor1.getPosition();
errA=refA-ticka;
Ap=errA*kpc;
Ad=(errA-Ad1)*kdc/ts;
@@ -194,7 +196,7 @@
Ai1=Ai;
ctrlA=(Ai+Ap+Ad);
for_A=(ctrlA)/1000.0;
- if(ctrlA<0) { //NOTE: als ticka weer +get is, kan deze weer 0 else 1 zijn.
+ if(ctrlA<0) {
Adir=1;
} else {
Adir=0;
@@ -202,11 +204,10 @@
keep_in_range(&for_A, -1,1);
if (frictionflag==true) {
for_A=abs(for_A)+friction;
- keep_in_range(&for_A, 0,0.08);
+ keep_in_range(&for_A, 0,0.1);
}
pwm_A.write(abs(for_A));
motordirA.write(Adir);
- //pc.printf("A %f %i \n\r",for_A,errA);
if(errA<20) {
calA=false;
pwm_A.write(0);
@@ -218,6 +219,7 @@
//Loop.
wait(1);
+ blueled.write(1.0);
while(meetflag==true) {
while(looptimerflag != true);
{
@@ -273,7 +275,7 @@
yabs1bl=yabsbl;
//Gains om filter te compenseren.
- zx=(zbr*gain);
+ zx=((zbr)*gain);
zy=(zbl*gain);
//Grenzen voor emg.
@@ -298,37 +300,41 @@
if (ztr>0.15 && dirflagx == true) {
dirflagx = false;
xdir ^= 1;
- //zx=0; //NOTE: deze weghalen kan schelen?
dirtimeout.attach(tricheck,1.0);
}
if (ztl>0.15 && dirflagy == true) {
dirflagy = false;
ydir ^= 1;
- //zy=0;
dirtimeout.attach(tricheck,1.0);
}
//Motoraansturing.
if (ydir==0) {
zy=-1.0*zy;
+ redled.write(1);
}
- if (xdir==0) { //NOTE: moet dit geen 0 zijn?
+ if (ydir==1){
+ redled.write(0);
+ }
+ if (xdir==0) {
zx=-1.0*zx;
+ greenled.write(1);
+ }
+ if (xdir==1) {
+ greenled.write(0);
}
- ticka=-1*motor1.getPosition(); //NOTE: -1 weghalen?
+ ticka=-1*motor1.getPosition();
tickb=motor2.getPosition();
//Begrenzing.
- vxuit=zx*20.0/500.0; //NOTE: deze shizzle kan er misschien uit. In ieder geval de vier. En anders moet het /1000 zijn.
- vyuit=zy*20.0/500.0;
+ vxuit=zx*70.0/500.0;
+ vyuit=zy*50.0/500.0;
xuit += ts*vxuit;
yuit += ts*vyuit;
- //keep_in_range(&xuit,-4.5,2.75); //NOTE: Iets met de grenzen. En de richtingpins.
- //keep_in_range(&yuit,-4.5,2.8); //NOTE: als bovenstaande weer klopt moeten de grenzen voor x zijn: .125 en 0.422. Voor y is dat 0.125 en 0.335.
- keep_in_range(&xuit,0.125,0.422);
- keep_in_range(&yuit,0.125,0.335);
+ keep_in_range(&xuit,0.115,0.422);
+ keep_in_range(&yuit,0.115,0.335);
refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
@@ -342,12 +348,10 @@
Ap=errA*kp;
Ad=(errA-Ad1)*kd/ts;
Ai=(Ai1+ts*errA)*ki;
- //keep_in_range(&Ad,-0.1,0.1);
- //keep_in_range(&Ai,-0.1,0.1);
Ad1=Ad;
Ai1=Ai;
ctrlA=(Ai+Ap+Ad);
- for_A=(ctrlA)/700.0; //NOTE: maak eens een plot van ctrlA en ctrlB. Kan leerzaam zijn voor deze gains.
+ for_A=(ctrlA)/10.0; //NOTE: maak eens een plot van ctrlA en ctrlB. Kan leerzaam zijn voor deze gains.
Bp=errB*kp;
Bd=(errB-Bd1)*kd/ts;
@@ -357,7 +361,7 @@
Bd1=Bd;
Bi1=Bi;
ctrlB=(Bi+Bp+Bd);
- for_B=(ctrlB)/7000.0;
+ for_B=(ctrlB)/1500.0;
//x en y uit emg in meters. IK naar radialen. dan radialen naar ticks.
if(ctrlA<0) { //NOTE: zeker gezien de vorige omdraaiingen moet deze volgens mij weer 0 else 1 zijn.
@@ -378,8 +382,8 @@
for_A=abs(for_A)+friction;
for_B=abs(for_B)+friction;
- keep_in_range(&for_A, friction,1.0); //NOTE: misschien valt hier nog wat te begrenzen, maar dan wel op ondergrens friction.
- keep_in_range(&for_B, friction,1.0);
+ keep_in_range(&for_A, friction,friction+0.2); //NOTE: misschien valt hier nog wat te begrenzen, maar dan wel op ondergrens friction.
+ keep_in_range(&for_B, friction,friction+0.2);
}
motordirA.write(Adir);
motordirB.write(Bdir);
@@ -388,10 +392,10 @@
if(pc.txBufferGetSize(0)-pc.txBufferGetCount() > 100) {
//pc.printf(" %f %f \n\r",zx, zy);
- //pc.printf(" %f %f %f %f %f \n\r",zbl,ztl,zbr,ztr,kbl);
+ pc.printf(" %f %f %f %f\n\r",zbl,ztl,zbr,ztr);
//pc.printf(" %f %i %i %f %i %i\n\r",for_A,ticka,refA,for_B,tickb, refB);
- pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit*1000.0,yuit*1000.0);
- //pc.printf("A %i %i B %i %i\n\r",Adir, ticka,Bdir, tickb);
+ //pc.printf(" %i %i %i %i %f %f \n\r", ticka,tickb,refA,refB,xuit*1000.0,yuit*1000.0);
+ //pc.printf("A %f B %f\n\r",ctrlA,ctrlB);
//pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit,yuit);
//pc.printf(" %i %i %i %f\n\r",tickb,refB,errB,for_B);
//pc.printf(" %f %f %f %f \n\r",kbl,zy,vyuit, yuit);
@@ -457,6 +461,7 @@
motordirB.write(Bdir);
pwm_A.write(abs(for_A));
pwm_B.write(abs(for_B));
+
}
wait(1);
xuit=400.0;
