Ralph Lentink / Mbed 2 deprecated BO

hfdh

Dependencies:   Encoder MODSERIAL mbed

Fork of BO by Thomas Plaisier

main.cpp@5:31120c4c08f0, 2014-06-12 (annotated)

Committer:
Socrates
Date:
Thu Jun 12 12:59:32 2014 +0000
Revision:
5:31120c4c08f0
Parent:
4:7052ee491e12
Child:
6:7ff223862008
Voor MODSERIAL;

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