Ralph Lentink / Mbed 2 deprecated BO

hfdh

Dependencies:   Encoder MODSERIAL mbed

Fork of BO by Thomas Plaisier

main.cpp@4:7052ee491e12, 2014-06-12 (annotated)

Committer:
Socrates
Date:
Thu Jun 12 12:17:23 2014 +0000
Revision:
4:7052ee491e12
Parent:
3:123f3fd0daf6
Child:
5:31120c4c08f0
Voor implementeren .h voor ref en tijd;

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 2:9dc7b40286d1 3 #include "MODSERIAL.h"
Socrates 2:9dc7b40286d1 4 #include "time.h"
Socrates 4:7052ee491e12 5 #include "ref.h"
Socrates 0:095ff84c3ee9 6
Socrates 2:9dc7b40286d1 7 MODSERIAL pc(USBTX,USBRX);
Socrates 0:095ff84c3ee9 8 Encoder Enc(PTA1,PTA2);
Socrates 0:095ff84c3ee9 9 PwmOut motorplus(PTD3);
Socrates 0:095ff84c3ee9 10 PwmOut motormin(PTC3);
Socrates 0:095ff84c3ee9 11
Socrates 1:95dd5c626960 12 double rpm, pos, pos1, t, Ref, Error, deliver;
Socrates 1:95dd5c626960 13 double Prop, Kp, Int, Int1, Ki, Ctrl;
Socrates 2:9dc7b40286d1 14 double pwmotor;
Socrates 2:9dc7b40286d1 15 double speed, move;
Socrates 2:9dc7b40286d1 16 volatile bool looptimerflag, calflag, scriptflag;
Socrates 0:095ff84c3ee9 17 double Move[25];
Socrates 1:95dd5c626960 18 double Tijd[9];
Socrates 1:95dd5c626960 19 double Referentie[9];
Socrates 3:123f3fd0daf6 20 int n;
Socrates 0:095ff84c3ee9 21
Socrates 0:095ff84c3ee9 22 void setlooptimerflag(void)
Socrates 0:095ff84c3ee9 23 {
Socrates 0:095ff84c3ee9 24 looptimerflag = true;
Socrates 0:095ff84c3ee9 25 }
Socrates 0:095ff84c3ee9 26 void setscriptflag(void)
Socrates 0:095ff84c3ee9 27 {
Socrates 0:095ff84c3ee9 28 scriptflag = true;
Socrates 0:095ff84c3ee9 29 }
Socrates 2:9dc7b40286d1 30 void setcalflag(void)
Socrates 2:9dc7b40286d1 31 {
Socrates 2:9dc7b40286d1 32 calflag = true;
Socrates 2:9dc7b40286d1 33 }
Socrates 0:095ff84c3ee9 34 void printdata(void)
Socrates 0:095ff84c3ee9 35 {
Socrates 3:123f3fd0daf6 36 pc.printf("Speed %f Pos %f del %f\n\r",speed, pos, deliver);
Socrates 0:095ff84c3ee9 37 }
Socrates 2:9dc7b40286d1 38 void printdata2(void)
Socrates 2:9dc7b40286d1 39 {
Socrates 3:123f3fd0daf6 40 pc.printf("Del %f Ctrl %f\n\r",deliver,Ctrl);
Socrates 3:123f3fd0daf6 41 }
Socrates 3:123f3fd0daf6 42 void printdata3(void)
Socrates 3:123f3fd0daf6 43 {
Socrates 3:123f3fd0daf6 44 pc.printf("P %f R %f\n\r",pos, Ref);
Socrates 2:9dc7b40286d1 45 }
Socrates 0:095ff84c3ee9 46
Socrates 0:095ff84c3ee9 47 int main()
Socrates 0:095ff84c3ee9 48 {
Socrates 1:95dd5c626960 49 pc.baud(115200);
Socrates 1:95dd5c626960 50 wait(1);
Socrates 1:95dd5c626960 51 pc.printf("start\n\r");
Socrates 1:95dd5c626960 52 wait(1);
Socrates 1:95dd5c626960 53 //Inladen data. Courtesy of http://runnable.com/UpSpP5ymsslIAAFo/reading-a-file-into-an-array-for-c%2B%2B-and-fstream
Socrates 1:95dd5c626960 54
Socrates 1:95dd5c626960 55 //Nu gaan we weer verder met het normale programma
Socrates 0:095ff84c3ee9 56 Ticker looptimer;
Socrates 0:095ff84c3ee9 57 Ticker printer;
Socrates 0:095ff84c3ee9 58 Ticker script;
Socrates 2:9dc7b40286d1 59 Ticker calibratie;
Socrates 2:9dc7b40286d1 60 double ts=(1.0/5000.0);
Socrates 2:9dc7b40286d1 61 double tsd=(1.0/5.0);
Socrates 0:095ff84c3ee9 62 motorplus.period(1.0/60000.0);
Socrates 0:095ff84c3ee9 63 motormin.period(1.0/60000.0);
Socrates 0:095ff84c3ee9 64
Socrates 4:7052ee491e12 65 Kp = 1.1471/1;
Socrates 4:7052ee491e12 66 Ki = 4.9157/1;
Socrates 3:123f3fd0daf6 67 pwmotor=-0.9/2.0;
Socrates 1:95dd5c626960 68 deliver=0.01*pwmotor;
Socrates 0:095ff84c3ee9 69 pos1=0;
Socrates 0:095ff84c3ee9 70 n=0;
Socrates 2:9dc7b40286d1 71 t=0;
Socrates 1:95dd5c626960 72 Move[24]=pwmotor*-100.0;
Socrates 2:9dc7b40286d1 73 pos=Enc.getPosition();
Socrates 2:9dc7b40286d1 74 speed=Enc.getSpeed();
Socrates 1:95dd5c626960 75
Socrates 4:7052ee491e12 76 /*
Socrates 1:95dd5c626960 77 Referentie[0]=20.0;
Socrates 1:95dd5c626960 78 Referentie[1]=17.0;
Socrates 1:95dd5c626960 79 Referentie[2]=14.0;
Socrates 1:95dd5c626960 80 Referentie[3]=20.0;
Socrates 3:123f3fd0daf6 81 Referentie[4]=16.0;
Socrates 1:95dd5c626960 82 Referentie[5]=20.0;
Socrates 1:95dd5c626960 83 Referentie[6]=14.0;
Socrates 3:123f3fd0daf6 84 Referentie[7]=25.0;
Socrates 1:95dd5c626960 85 Referentie[8]=23.0;
Socrates 4:7052ee491e12 86 */
Socrates 4:7052ee491e12 87 /*
Socrates 1:95dd5c626960 88 Tijd[0]=0;
Socrates 1:95dd5c626960 89 Tijd[1]=5.5;
Socrates 1:95dd5c626960 90 Tijd[2]=7.0;
Socrates 1:95dd5c626960 91 Tijd[3]=9.25;
Socrates 1:95dd5c626960 92 Tijd[4]=12.0;
Socrates 1:95dd5c626960 93 Tijd[5]=15.5;
Socrates 1:95dd5c626960 94 Tijd[6]=17.5;
Socrates 1:95dd5c626960 95 Tijd[7]=23.0;
Socrates 1:95dd5c626960 96 Tijd[8]=25.0;
Socrates 4:7052ee491e12 97 */
Socrates 2:9dc7b40286d1 98 wait(1);
Socrates 2:9dc7b40286d1 99 pc.printf("start2\n\r");
Socrates 1:95dd5c626960 100 wait(1);
Socrates 0:095ff84c3ee9 101
Socrates 2:9dc7b40286d1 102 scriptflag = true;
Socrates 2:9dc7b40286d1 103 printer.attach(printdata,tsd);
Socrates 2:9dc7b40286d1 104 //script.attach(setscriptflag, ts);
Socrates 2:9dc7b40286d1 105 calibratie.attach(setcalflag,ts);
Socrates 1:95dd5c626960 106 while(scriptflag == true) {
Socrates 0:095ff84c3ee9 107 while(calflag == true) {
Socrates 2:9dc7b40286d1 108 calflag=false;
Socrates 1:95dd5c626960 109 if (abs(deliver) <= abs(pwmotor)) {
Socrates 1:95dd5c626960 110 deliver=deliver*1.01;
Socrates 1:95dd5c626960 111 }
Socrates 1:95dd5c626960 112 motorplus.write(0.5+deliver);
Socrates 1:95dd5c626960 113 motormin.write(0.5-deliver);
Socrates 1:95dd5c626960 114 //pc.printf("pwm %f\n\r",deliver);
Socrates 0:095ff84c3ee9 115 speed=Enc.getSpeed();
Socrates 0:095ff84c3ee9 116 pos1=pos;
Socrates 0:095ff84c3ee9 117 pos=Enc.getPosition()/1024.0;
Socrates 0:095ff84c3ee9 118 Move[n]=pos-pos1;
Socrates 0:095ff84c3ee9 119 n += 1;
Socrates 0:095ff84c3ee9 120 if (n >24) {
Socrates 0:095ff84c3ee9 121 n=0;
Socrates 0:095ff84c3ee9 122 }
Socrates 0:095ff84c3ee9 123 move=Move[24]-Move[0];
Socrates 2:9dc7b40286d1 124 pc.printf("Pwm %f Pos %f\n\r",deliver,pos);
Socrates 2:9dc7b40286d1 125 if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 0.5) {
Socrates 2:9dc7b40286d1 126 motorplus.write(0.5);
Socrates 2:9dc7b40286d1 127 motormin.write(0.5);
Socrates 1:95dd5c626960 128 //pwmotor = pwmotor*-1.0;
Socrates 1:95dd5c626960 129 //deliver=0.01*pwmotor;
Socrates 2:9dc7b40286d1 130 pc.printf("Einde\n\r");
Socrates 2:9dc7b40286d1 131 calibratie.detach();
Socrates 2:9dc7b40286d1 132 printer.detach();
Socrates 1:95dd5c626960 133 calflag=false;
Socrates 2:9dc7b40286d1 134 scriptflag=false;
Socrates 1:95dd5c626960 135 wait(1);
Socrates 0:095ff84c3ee9 136 }
Socrates 0:095ff84c3ee9 137 }
Socrates 0:095ff84c3ee9 138 }
Socrates 1:95dd5c626960 139 Enc.setPosition(0.0);
Socrates 2:9dc7b40286d1 140 pwmotor=0.0;
Socrates 3:123f3fd0daf6 141 deliver=0.01;
Socrates 2:9dc7b40286d1 142 pc.printf("Begin loop\n\r");
Socrates 2:9dc7b40286d1 143 wait(1);
Socrates 3:123f3fd0daf6 144 printer.attach(printdata3,tsd);
Socrates 0:095ff84c3ee9 145 looptimer.attach(setlooptimerflag,ts);
Socrates 3:123f3fd0daf6 146 Ref=Referentie[0];
Socrates 2:9dc7b40286d1 147 while(1) {
Socrates 3:123f3fd0daf6 148 while (looptimerflag != true) {}
Socrates 3:123f3fd0daf6 149 looptimerflag = false;
Socrates 1:95dd5c626960 150 //Encoder
Socrates 3:123f3fd0daf6 151 pos=Enc.getPosition()/1024;
Socrates 3:123f3fd0daf6 152 speed=Enc.getSpeed();
Socrates 0:095ff84c3ee9 153
Socrates 3:123f3fd0daf6 154 if (t >= Tijd[n]) {
Socrates 3:123f3fd0daf6 155 Ref=Referentie[n];
Socrates 3:123f3fd0daf6 156 n +=1;
Socrates 3:123f3fd0daf6 157 if (n >=7) {
Socrates 3:123f3fd0daf6 158 n=7;
Socrates 2:9dc7b40286d1 159 }
Socrates 3:123f3fd0daf6 160 }
Socrates 3:123f3fd0daf6 161 t = t + ts;
Socrates 0:095ff84c3ee9 162
Socrates 1:95dd5c626960 163 //Regelaar
Socrates 3:123f3fd0daf6 164 Error=Ref-pos;
Socrates 3:123f3fd0daf6 165 Prop=Error*Kp;
Socrates 3:123f3fd0daf6 166 Int1=Int;
Socrates 4:7052ee491e12 167 Int=(Int1+0.01*ts*Error)*Ki;
Socrates 3:123f3fd0daf6 168 Ctrl=Prop+Int;
Socrates 1:95dd5c626960 169
Socrates 3:123f3fd0daf6 170 pwmotor=Ctrl;
Socrates 3:123f3fd0daf6 171 deliver=pwmotor;
Socrates 3:123f3fd0daf6 172 motorplus.write(0.5+deliver);
Socrates 3:123f3fd0daf6 173 motormin.write(0.5-deliver);
Socrates 3:123f3fd0daf6 174 if (t >= 30.0) {
Socrates 3:123f3fd0daf6 175 motorplus.write(0.0);
Socrates 3:123f3fd0daf6 176 motormin.write(0.0);
Socrates 4:7052ee491e12 177 pc.printf("\n\rBeindigd door tijd");
Socrates 3:123f3fd0daf6 178 return 0;
Socrates 3:123f3fd0daf6 179 }
Socrates 3:123f3fd0daf6 180 if (abs(pos) >= 30 || (t >=10.0 && pos <=0.5)) {
Socrates 3:123f3fd0daf6 181 motorplus.write(0.0);
Socrates 3:123f3fd0daf6 182 motormin.write(0.0);
Socrates 4:7052ee491e12 183 pc.printf("\n\rBeindigd door positie\n\r");
Socrates 3:123f3fd0daf6 184 return 0;
Socrates 2:9dc7b40286d1 185 }
Socrates 1:95dd5c626960 186 }
Socrates 0:095ff84c3ee9 187 }