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:
- 34:961aec0a13c6
- Parent:
- 33:c495e9d8ea1f
- Child:
- 35:db3385976119
--- a/main.cpp Mon Nov 04 15:27:44 2013 +0000
+++ b/main.cpp Mon Nov 04 16:26:02 2013 +0000
@@ -45,8 +45,8 @@
volatile bool frictionflag=true;
volatile bool calflag=true;
-volatile bool meetflag=true;
-volatile bool patroonflag=false;
+volatile bool meetflag=false;
+volatile bool patroonflag=true;
int main()
{
@@ -69,6 +69,8 @@
int xdir, ydir, Adir, Bdir;
int ticka, tickb, refA,refB, errA, errB;
int Aboven, Aonder, Bboven,Bonder;
+
+ int ppos=1;
//Startwaarden.
x1tr=0;
y1tr=0;
@@ -134,13 +136,6 @@
//denl1=1;
denl2=-1.822694925196308;
denl3=0.837181651256023;
- //Low pass, 2 Hz, 2e orde, 1 ms.
- //numl1=0.391302053991682*pow(10.0,-4.0);
- //numl2=0.782604107983365*pow(10.0,-4.0);
- //numl3=0.391302053991682*pow(10.0,-4.0);
- //denl1=1;
- //denl2=-1.982228929792529;
- //denl3=0.982385450614126;
//Opzetje voor calibratie
wait(4);
@@ -148,7 +143,7 @@
if(startflag==true);
{
motor1.setPosition(200);
- motor2.setPosition(1200);
+ motor2.setPosition(1100);
startflag=false;
}
@@ -275,7 +270,7 @@
yabs1bl=yabsbl;
//Gains om filter te compenseren.
- zx=((zbr)*gain);
+ zx=(zbr*gain);
zy=(zbl*gain);
//Grenzen voor emg.
@@ -313,15 +308,15 @@
zy=-1.0*zy;
redled.write(1);
}
- if (ydir==1){
- redled.write(0);
+ if (ydir==1) {
+ redled.write(0);
}
if (xdir==0) {
zx=-1.0*zx;
greenled.write(1);
}
if (xdir==1) {
- greenled.write(0);
+ greenled.write(0);
}
ticka=-1*motor1.getPosition();
@@ -329,7 +324,7 @@
//Begrenzing.
vxuit=zx*70.0/500.0;
- vyuit=zy*50.0/500.0;
+ vyuit=zy*50.0/500.0;
xuit += ts*vxuit;
yuit += ts*vyuit;
@@ -351,20 +346,17 @@
Ad1=Ad;
Ai1=Ai;
ctrlA=(Ai+Ap+Ad);
- for_A=(ctrlA)/10.0; //NOTE: maak eens een plot van ctrlA en ctrlB. Kan leerzaam zijn voor deze gains.
+ for_A=(ctrlA)/10.0;
Bp=errB*kp;
Bd=(errB-Bd1)*kd/ts;
Bi=(Bi1+ts*errB)*ki;
- //keep_in_range(&Bd,-0.1,0.1);
- //keep_in_range(&Bi,-0.1,0.1);
Bd1=Bd;
Bi1=Bi;
ctrlB=(Bi+Bp+Bd);
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.
+ if(ctrlA<0) {
Adir=1;
} else {
Adir=0;
@@ -382,7 +374,7 @@
for_A=abs(for_A)+friction;
for_B=abs(for_B)+friction;
- 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_A, friction,friction+0.2);
keep_in_range(&for_B, friction,friction+0.2);
}
motordirA.write(Adir);
@@ -409,114 +401,87 @@
while(patroonflag==true) {
while(looptimerflag != true);
looptimerflag = false;
- xuit=400.0;
- yuit=140.0;
+
+ if(ppos==1) {
+ refA=221;
+ refB=8477;
+ //xuit=0.400;
+ //yuit=0.140;
+ }
+ if(ppos==2) {
+ refA=278;
+ refB=8774;
+ //xuit=0.400;
+ //yuit=0.180;
+ }
+ if(ppos==3) {
+ //xuit=0.400;
+ //yuit=0.200;
+ }
+ if(ppos==4) {
+ //xuit=0.340;
+ //yuit=0.200;
+ }
+ if(ppos==5) {
+ //xuit=0.340;
+ //yuit=0.180;
+ }
+ if(ppos==6) {
+ //xuit=0.350;
+ //yuit=0.200;
+ }
ticka=-1*motor1.getPosition();
tickb=motor2.getPosition();
- refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
- refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt*1000.0);
+ //refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
+ //refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt*1000.0);
errA=refA-ticka;
errB=refB-tickb;
- while(errA>20 && errB>20) {
- ticka=-1*motor1.getPosition();
- tickb=motor2.getPosition();
- refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
- refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt);
- errA=refA-ticka;
- errB=refB-tickb;
- //Controllers
- Ap=errA*kpc;
- Ad=(errA-Ad1)*kdc/ts;
- Ai=(Ai1+ts*errA)*kic;
- Ad1=Ad;
- Ai1=Ai;
- ctrlA=(Ai+Ap+Ad);
- for_A=(ctrlA)/1000.0;
- Bp=errB*kpc;
- Bd=(errB-Bd1)*kdc/ts;
- Bi=(Bi1+ts*errB)*kic;
- Bd1=Bd;
- Bi1=Bi;
- ctrlB=(Bi+Bp+Bd);
- for_B=(ctrlB)/1000.0;
- if(ctrlA<0) {
- Adir=0;
- } else {
- Adir=1;
- }
- if(ctrlB<0) {
- Bdir=0;
- } else {
- Bdir=1;
- }
- keep_in_range(&for_A, -1.0,1.0);
- keep_in_range(&for_B, -1.0,1.0);
- if (frictionflag==true) {
- for_A=abs(for_A)+friction;
- for_B=abs(for_B)+friction;
- keep_in_range(&for_A, 0,0.08);
- keep_in_range(&for_B, 0,0.1);
- }
- motordirA.write(Adir);
- motordirB.write(Bdir);
- pwm_A.write(abs(for_A));
- pwm_B.write(abs(for_B));
-
+ //Controllers
+ Ap=errA*kpc;
+ Ad=(errA-Ad1)*kdc/ts;
+ Ai=(Ai1+ts*errA)*kic;
+ Ad1=Ad;
+ Ai1=Ai;
+ ctrlA=(Ai+Ap+Ad);
+ for_A=(ctrlA)/100.0;
+ Bp=errB*kpc;
+ Bd=(errB-Bd1)*kdc/ts;
+ Bi=(Bi1+ts*errB)*kic;
+ Bd1=Bd;
+ Bi1=Bi;
+ ctrlB=(Bi+Bp+Bd);
+ for_B=(ctrlB)/100.0;
+ if(ctrlA<0) {
+ Adir=1;
+ } else {
+ Adir=0;
}
- wait(1);
- xuit=400.0;
- yuit=180.0;
- while(errA>20 && errB>20) {
- ticka=-1*motor1.getPosition();
- tickb=motor2.getPosition();
- refA=4123.0*atan2(yuit,xuit)/(2.0*PI);
- refB=4123.0*sqrt(xuit*xuit+yuit*yuit)/(2.0*PI*rt*1000.0);
- //keep_in_rangeint(&refA,Aonder,Aboven);
- //keep_in_rangeint(&refB,Bonder,Bboven);
- errA=refA-ticka;
- errB=refB-tickb;
- //Controllers
- Ap=errA*kpc;
- Ad=(errA-Ad1)*kdc/ts;
- Ai=(Ai1+ts*errA)*kic;
- Ad1=Ad;
- Ai1=Ai;
- ctrlA=(Ai+Ap+Ad);
- for_A=(ctrlA)/1000.0;
- Bp=errB*kpc;
- Bd=(errB-Bd1)*kdc/ts;
- Bi=(Bi1+ts*errB)*kic;
- Bd1=Bd;
- Bi1=Bi;
- ctrlB=(Bi+Bp+Bd);
- for_B=(ctrlB)/1000.0;
- if(ctrlA<0) {
- Adir=0;
- } else {
- Adir=1;
- }
- if(ctrlB<0) {
- Bdir=0;
- } else {
- Bdir=1;
- }
- keep_in_range(&for_A, -1.0,1.0);
- keep_in_range(&for_B, -1.0,1.0);
- if (frictionflag==true) {
- for_A=abs(for_A)+friction;
- for_B=abs(for_B)+friction;
- keep_in_range(&for_A, 0,0.08);
- keep_in_range(&for_B, 0,0.1);
- }
- motordirA.write(Adir);
- motordirB.write(Bdir);
- pwm_A.write(abs(for_A));
- pwm_B.write(abs(for_B));
+ if(ctrlB<0) {
+ Bdir=0;
+ } else {
+ Bdir=1;
+ }
+ keep_in_range(&for_A, -1.0,1.0);
+ keep_in_range(&for_B, -1.0,1.0);
+ if (frictionflag==true) {
+ for_A=abs(for_A)+friction;
+ for_B=abs(for_B)+friction;
+ keep_in_range(&for_A, 0,0.5);
+ keep_in_range(&for_B, 0,0.6);
}
- }
+ motordirA.write(Adir);
+ motordirB.write(Bdir);
+ pwm_A.write(abs(for_A));
+ pwm_B.write(abs(for_B));
+ if(errA<40 && errB<40) {
+ ppos +=1;
+ wait(1);
+ }
+ }
}
+
void keep_in_range(double * in, double min, double max)
{
*in > min ? *in < max? : *in = max: *in = min;
