Ralph Lentink / Mbed 2 deprecated BO

hfdh

Dependencies:   Encoder MODSERIAL mbed

Fork of BO by Thomas Plaisier

main.cpp@0:095ff84c3ee9, 2014-06-11 (annotated)

Committer:
Socrates
Date:
Wed Jun 11 08:22:59 2014 +0000
Revision:
0:095ff84c3ee9
Child:
1:95dd5c626960
Kopie van tau op 10-6-14

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Socrates 0:095ff84c3ee9 1 #include "mbed.h"
Socrates 0:095ff84c3ee9 2 #include "encoder.h"
Socrates 0:095ff84c3ee9 3
Socrates 0:095ff84c3ee9 4 Serial pc(USBTX,USBRX);
Socrates 0:095ff84c3ee9 5 Encoder Enc(PTA1,PTA2);
Socrates 0:095ff84c3ee9 6 Encoder Encinv(PTA0,PTD0);
Socrates 0:095ff84c3ee9 7 PwmOut motorplus(PTD3);
Socrates 0:095ff84c3ee9 8 PwmOut motormin(PTC3);
Socrates 0:095ff84c3ee9 9
Socrates 0:095ff84c3ee9 10 double rpm, rpmmin,pos,pos1,posinv;
Socrates 0:095ff84c3ee9 11 volatile double pwmotor;
Socrates 0:095ff84c3ee9 12 double tickinv, tick, speed,move, speedinv;
Socrates 0:095ff84c3ee9 13 volatile bool looptimerflag, calflag, scriptflag;
Socrates 0:095ff84c3ee9 14 double Move[25];
Socrates 0:095ff84c3ee9 15 int n;
Socrates 0:095ff84c3ee9 16
Socrates 0:095ff84c3ee9 17 void setlooptimerflag(void)
Socrates 0:095ff84c3ee9 18 {
Socrates 0:095ff84c3ee9 19 looptimerflag = true;
Socrates 0:095ff84c3ee9 20 }
Socrates 0:095ff84c3ee9 21 void setscriptflag(void)
Socrates 0:095ff84c3ee9 22 {
Socrates 0:095ff84c3ee9 23 scriptflag = true;
Socrates 0:095ff84c3ee9 24 }
Socrates 0:095ff84c3ee9 25 void printdata(void)
Socrates 0:095ff84c3ee9 26 {
Socrates 0:095ff84c3ee9 27 pc.printf("Speed %f Pos %f\n\r",speed, pos);
Socrates 0:095ff84c3ee9 28 }
Socrates 0:095ff84c3ee9 29
Socrates 0:095ff84c3ee9 30 int main()
Socrates 0:095ff84c3ee9 31 {
Socrates 0:095ff84c3ee9 32 Ticker looptimer;
Socrates 0:095ff84c3ee9 33 Ticker printer;
Socrates 0:095ff84c3ee9 34 Ticker script;
Socrates 0:095ff84c3ee9 35 const double ts=(1.0/5000.0);
Socrates 0:095ff84c3ee9 36 const double tsd=(1.0/5.0);
Socrates 0:095ff84c3ee9 37 //20 kHz kan hij nog aan, in een simpel script.
Socrates 0:095ff84c3ee9 38 motorplus.period(1.0/60000.0);
Socrates 0:095ff84c3ee9 39 motormin.period(1.0/60000.0);
Socrates 0:095ff84c3ee9 40 //Die moet duidelijk gefilterd worden.
Socrates 0:095ff84c3ee9 41 pc.baud(115200);
Socrates 0:095ff84c3ee9 42
Socrates 0:095ff84c3ee9 43 pwmotor=0.3/2.0;
Socrates 0:095ff84c3ee9 44 pos1=0;
Socrates 0:095ff84c3ee9 45 n=0;
Socrates 0:095ff84c3ee9 46 Move[24]=-10;
Socrates 0:095ff84c3ee9 47 calflag = true;
Socrates 0:095ff84c3ee9 48
Socrates 0:095ff84c3ee9 49 printer.attach(printdata,tsd);
Socrates 0:095ff84c3ee9 50 wait(tsd);
Socrates 0:095ff84c3ee9 51
Socrates 0:095ff84c3ee9 52 script.attach(setscriptflag, ts);
Socrates 0:095ff84c3ee9 53 while(scripttflag == true) {
Socrates 0:095ff84c3ee9 54 scriptflag = false;
Socrates 0:095ff84c3ee9 55 wait(3);
Socrates 0:095ff84c3ee9 56 while(calflag == true) {
Socrates 0:095ff84c3ee9 57 motorplus.write(0.5+pwmotor);
Socrates 0:095ff84c3ee9 58 motormin.write(0.5-pwmotor);
Socrates 0:095ff84c3ee9 59 speed=Enc.getSpeed();
Socrates 0:095ff84c3ee9 60 pos1=pos;
Socrates 0:095ff84c3ee9 61 pos=Enc.getPosition()/1024.0;
Socrates 0:095ff84c3ee9 62 Move[n]=pos-pos1;
Socrates 0:095ff84c3ee9 63 n += 1;
Socrates 0:095ff84c3ee9 64 if (n >24) {
Socrates 0:095ff84c3ee9 65 n=0;
Socrates 0:095ff84c3ee9 66 }
Socrates 0:095ff84c3ee9 67 move=Move[24]-Move[0];
Socrates 0:095ff84c3ee9 68 if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 10) {
Socrates 0:095ff84c3ee9 69 motorplus.write(0.0);
Socrates 0:095ff84c3ee9 70 motormin.write(0.0);
Socrates 0:095ff84c3ee9 71 pwmotor = pwmotor*-1;
Socrates 0:095ff84c3ee9 72 }
Socrates 0:095ff84c3ee9 73 }
Socrates 0:095ff84c3ee9 74 script.detach();
Socrates 0:095ff84c3ee9 75 }
Socrates 0:095ff84c3ee9 76
Socrates 0:095ff84c3ee9 77 looptimer.attach(setlooptimerflag,ts);
Socrates 0:095ff84c3ee9 78 while(looptimerflag == false);
Socrates 0:095ff84c3ee9 79 {
Socrates 0:095ff84c3ee9 80 motorplus.write(0.5+pwmotor);
Socrates 0:095ff84c3ee9 81 motormin.write(0.5-pwmotor);
Socrates 0:095ff84c3ee9 82 }
Socrates 0:095ff84c3ee9 83 looptimerflag = false;
Socrates 0:095ff84c3ee9 84
Socrates 0:095ff84c3ee9 85 //Encoder
Socrates 0:095ff84c3ee9 86 tick=Enc.getPosition();
Socrates 0:095ff84c3ee9 87 //tickinv=Encinv.getPosition();
Socrates 0:095ff84c3ee9 88 speed=Enc.getSpeed();
Socrates 0:095ff84c3ee9 89 //speedinv=Encinv.getSpeed();
Socrates 0:095ff84c3ee9 90 pos=tick/1024;
Socrates 0:095ff84c3ee9 91 //posinv=tickinv/1024;
Socrates 0:095ff84c3ee9 92
Socrates 0:095ff84c3ee9 93 t += ts;
Socrates 0:095ff84c3ee9 94 if (t==Tijd[n]) {
Socrates 0:095ff84c3ee9 95 Ref=Referentie[n];
Socrates 0:095ff84c3ee9 96 n +=1;
Socrates 0:095ff84c3ee9 97 }
Socrates 0:095ff84c3ee9 98
Socrates 0:095ff84c3ee9 99 //Regelaar
Socrates 0:095ff84c3ee9 100 Error=Ref-pos;
Socrates 0:095ff84c3ee9 101 Prop=Error*Kp;
Socrates 0:095ff84c3ee9 102 Int=(Int1+ts*Error)*Ki;
Socrates 0:095ff84c3ee9 103 Ctrl=Prop+Int;
Socrates 0:095ff84c3ee9 104 pwmotor=Ctrl;
Socrates 0:095ff84c3ee9 105
Socrates 0:095ff84c3ee9 106 motorplus.write(0.5+pwmotor);
Socrates 0:095ff84c3ee9 107 motormin.write(0.5-pwmotor);
Socrates 0:095ff84c3ee9 108 //pc.printf("RPM %f Y %f\n\r",rpm, y);
Socrates 0:095ff84c3ee9 109 }