Ralph Lentink / Mbed 2 deprecated BO

hfdh

Dependencies:   Encoder MODSERIAL mbed

Fork of BO by Thomas Plaisier

main.cpp@1:95dd5c626960, 2014-06-12 (annotated)

Committer:
Socrates
Date:
Thu Jun 12 07:16:37 2014 +0000
Revision:
1:95dd5c626960
Parent:
0:095ff84c3ee9
Child:
2:9dc7b40286d1
Voor verwijderen fstream (zinloos door gebrek aan sd-kaart).

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 1:95dd5c626960 3 #include <iostream>
Socrates 1:95dd5c626960 4 #include <fstream>
Socrates 1:95dd5c626960 5 using namespace std;
Socrates 0:095ff84c3ee9 6
Socrates 0:095ff84c3ee9 7 Serial pc(USBTX,USBRX);
Socrates 0:095ff84c3ee9 8 Encoder Enc(PTA1,PTA2);
Socrates 1:95dd5c626960 9 //Encoder Encinv(PTA0,PTD0);
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 0:095ff84c3ee9 15 volatile double pwmotor;
Socrates 1:95dd5c626960 16 double tick, speed, move;
Socrates 1:95dd5c626960 17 volatile bool looptimerflag, calflag, scriptflag, loadingflag;
Socrates 0:095ff84c3ee9 18 double Move[25];
Socrates 1:95dd5c626960 19 double Tijd[9];
Socrates 1:95dd5c626960 20 double Referentie[9];
Socrates 0:095ff84c3ee9 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 0:095ff84c3ee9 31 void printdata(void)
Socrates 0:095ff84c3ee9 32 {
Socrates 0:095ff84c3ee9 33 pc.printf("Speed %f Pos %f\n\r",speed, pos);
Socrates 0:095ff84c3ee9 34 }
Socrates 0:095ff84c3ee9 35
Socrates 0:095ff84c3ee9 36 int main()
Socrates 0:095ff84c3ee9 37 {
Socrates 1:95dd5c626960 38 pc.baud(115200);
Socrates 1:95dd5c626960 39 wait(1);
Socrates 1:95dd5c626960 40 pc.printf("start\n\r");
Socrates 1:95dd5c626960 41 wait(1);
Socrates 1:95dd5c626960 42 loadingflag = true;
Socrates 1:95dd5c626960 43 //Inladen data. Courtesy of http://runnable.com/UpSpP5ymsslIAAFo/reading-a-file-into-an-array-for-c%2B%2B-and-fstream
Socrates 1:95dd5c626960 44
Socrates 1:95dd5c626960 45 /*
Socrates 1:95dd5c626960 46 while(loadingflag == true) {
Socrates 1:95dd5c626960 47 int array_size = 2847;
Socrates 1:95dd5c626960 48 char * array = new char[array_size];
Socrates 1:95dd5c626960 49 int index = 0;
Socrates 1:95dd5c626960 50 ifstream file("/local/m3l5xascii.txt");
Socrates 1:95dd5c626960 51 if(file.is_open()) {
Socrates 1:95dd5c626960 52 cout << "Bestand geopend. Laden...";
Socrates 1:95dd5c626960 53 while(!file.eof() && index < array_size) {
Socrates 1:95dd5c626960 54 file.get(array[index]);
Socrates 1:95dd5c626960 55 index++;
Socrates 1:95dd5c626960 56 }
Socrates 1:95dd5c626960 57 array[index-1] = '\0';
Socrates 1:95dd5c626960 58 cout << "De data tot nu" << endl;
Socrates 1:95dd5c626960 59 for(int i = 0; array[i] != '\0'; i++) {
Socrates 1:95dd5c626960 60 cout <<array[i];
Socrates 1:95dd5c626960 61 }
Socrates 1:95dd5c626960 62 } else {
Socrates 1:95dd5c626960 63 cout << "Bestand kan niet worden geopend" << endl;
Socrates 1:95dd5c626960 64 }
Socrates 1:95dd5c626960 65 loadingflag = false;
Socrates 1:95dd5c626960 66 cout << array;
Socrates 1:95dd5c626960 67 return 0;
Socrates 1:95dd5c626960 68 }
Socrates 1:95dd5c626960 69 */
Socrates 1:95dd5c626960 70
Socrates 1:95dd5c626960 71 wait(1);
Socrates 1:95dd5c626960 72 //Nu gaan we weer verder met het normale programma
Socrates 0:095ff84c3ee9 73 Ticker looptimer;
Socrates 0:095ff84c3ee9 74 Ticker printer;
Socrates 0:095ff84c3ee9 75 Ticker script;
Socrates 0:095ff84c3ee9 76 const double ts=(1.0/5000.0);
Socrates 0:095ff84c3ee9 77 const double tsd=(1.0/5.0);
Socrates 0:095ff84c3ee9 78 //20 kHz kan hij nog aan, in een simpel script.
Socrates 0:095ff84c3ee9 79 motorplus.period(1.0/60000.0);
Socrates 0:095ff84c3ee9 80 motormin.period(1.0/60000.0);
Socrates 0:095ff84c3ee9 81 //Die moet duidelijk gefilterd worden.
Socrates 0:095ff84c3ee9 82
Socrates 1:95dd5c626960 83 Kp = 0.01;
Socrates 1:95dd5c626960 84 Ki = 0.01;
Socrates 1:95dd5c626960 85 pwmotor=-0.5/2.0;
Socrates 1:95dd5c626960 86 deliver=0.01*pwmotor;
Socrates 0:095ff84c3ee9 87 pos1=0;
Socrates 0:095ff84c3ee9 88 n=0;
Socrates 1:95dd5c626960 89 Move[24]=pwmotor*-100.0;
Socrates 1:95dd5c626960 90
Socrates 1:95dd5c626960 91 Referentie[0]=20.0;
Socrates 1:95dd5c626960 92 Referentie[1]=17.0;
Socrates 1:95dd5c626960 93 Referentie[2]=14.0;
Socrates 1:95dd5c626960 94 Referentie[3]=20.0;
Socrates 1:95dd5c626960 95 Referentie[4]=14.0;
Socrates 1:95dd5c626960 96 Referentie[5]=20.0;
Socrates 1:95dd5c626960 97 Referentie[6]=14.0;
Socrates 1:95dd5c626960 98 Referentie[7]=29.0;
Socrates 1:95dd5c626960 99 Referentie[8]=23.0;
Socrates 1:95dd5c626960 100
Socrates 1:95dd5c626960 101 Tijd[0]=0;
Socrates 1:95dd5c626960 102 Tijd[1]=5.5;
Socrates 1:95dd5c626960 103 Tijd[2]=7.0;
Socrates 1:95dd5c626960 104 Tijd[3]=9.25;
Socrates 1:95dd5c626960 105 Tijd[4]=12.0;
Socrates 1:95dd5c626960 106 Tijd[5]=15.5;
Socrates 1:95dd5c626960 107 Tijd[6]=17.5;
Socrates 1:95dd5c626960 108 Tijd[7]=23.0;
Socrates 1:95dd5c626960 109 Tijd[8]=25.0;
Socrates 0:095ff84c3ee9 110
Socrates 0:095ff84c3ee9 111 printer.attach(printdata,tsd);
Socrates 1:95dd5c626960 112 script.attach(setscriptflag, ts);
Socrates 1:95dd5c626960 113 pos=Enc.getPosition();
Socrates 1:95dd5c626960 114 speed=Enc.getSpeed();
Socrates 1:95dd5c626960 115 //pc.printf("start2");
Socrates 1:95dd5c626960 116 wait(1);
Socrates 1:95dd5c626960 117 calflag = true;
Socrates 0:095ff84c3ee9 118
Socrates 1:95dd5c626960 119 while(scriptflag == true) {
Socrates 0:095ff84c3ee9 120 scriptflag = false;
Socrates 0:095ff84c3ee9 121 wait(3);
Socrates 0:095ff84c3ee9 122 while(calflag == true) {
Socrates 1:95dd5c626960 123 if (abs(deliver) <= abs(pwmotor)) {
Socrates 1:95dd5c626960 124 deliver=deliver*1.01;
Socrates 1:95dd5c626960 125 }
Socrates 1:95dd5c626960 126 motorplus.write(0.5+deliver);
Socrates 1:95dd5c626960 127 motormin.write(0.5-deliver);
Socrates 1:95dd5c626960 128 //pc.printf("pwm %f\n\r",deliver);
Socrates 0:095ff84c3ee9 129 speed=Enc.getSpeed();
Socrates 0:095ff84c3ee9 130 pos1=pos;
Socrates 0:095ff84c3ee9 131 pos=Enc.getPosition()/1024.0;
Socrates 0:095ff84c3ee9 132 Move[n]=pos-pos1;
Socrates 0:095ff84c3ee9 133 n += 1;
Socrates 0:095ff84c3ee9 134 if (n >24) {
Socrates 0:095ff84c3ee9 135 n=0;
Socrates 0:095ff84c3ee9 136 }
Socrates 0:095ff84c3ee9 137 move=Move[24]-Move[0];
Socrates 1:95dd5c626960 138 pc.printf("pwm %f move %f\n\r",deliver,move);
Socrates 1:95dd5c626960 139 if(Move[24] == 0.0 && Move[0] == 0.0 && abs(pos) > 3) {
Socrates 0:095ff84c3ee9 140 motorplus.write(0.0);
Socrates 0:095ff84c3ee9 141 motormin.write(0.0);
Socrates 1:95dd5c626960 142 //pwmotor = pwmotor*-1.0;
Socrates 1:95dd5c626960 143 //deliver=0.01*pwmotor;
Socrates 1:95dd5c626960 144 calflag=false;
Socrates 1:95dd5c626960 145 pc.printf("Einde");
Socrates 1:95dd5c626960 146 script.detach();
Socrates 1:95dd5c626960 147 wait(1);
Socrates 0:095ff84c3ee9 148 }
Socrates 0:095ff84c3ee9 149 }
Socrates 0:095ff84c3ee9 150 }
Socrates 1:95dd5c626960 151 Enc.setPosition(0.0);
Socrates 0:095ff84c3ee9 152 looptimer.attach(setlooptimerflag,ts);
Socrates 1:95dd5c626960 153 pwmotor=0.0;
Socrates 0:095ff84c3ee9 154 while(looptimerflag == false);
Socrates 0:095ff84c3ee9 155 {
Socrates 1:95dd5c626960 156 motorplus.write(0.5+deliver);
Socrates 1:95dd5c626960 157 motormin.write(0.5-deliver);
Socrates 0:095ff84c3ee9 158 }
Socrates 0:095ff84c3ee9 159 looptimerflag = false;
Socrates 0:095ff84c3ee9 160
Socrates 1:95dd5c626960 161 //Encoder
Socrates 0:095ff84c3ee9 162 tick=Enc.getPosition();
Socrates 0:095ff84c3ee9 163 speed=Enc.getSpeed();
Socrates 0:095ff84c3ee9 164 pos=tick/1024;
Socrates 0:095ff84c3ee9 165
Socrates 0:095ff84c3ee9 166 if (t==Tijd[n]) {
Socrates 0:095ff84c3ee9 167 Ref=Referentie[n];
Socrates 0:095ff84c3ee9 168 n +=1;
Socrates 0:095ff84c3ee9 169 }
Socrates 1:95dd5c626960 170 t += ts;
Socrates 0:095ff84c3ee9 171
Socrates 1:95dd5c626960 172 //Regelaar
Socrates 0:095ff84c3ee9 173 Error=Ref-pos;
Socrates 0:095ff84c3ee9 174 Prop=Error*Kp;
Socrates 0:095ff84c3ee9 175 Int=(Int1+ts*Error)*Ki;
Socrates 0:095ff84c3ee9 176 Ctrl=Prop+Int;
Socrates 1:95dd5c626960 177
Socrates 0:095ff84c3ee9 178 pwmotor=Ctrl;
Socrates 1:95dd5c626960 179 if (abs(deliver) <= abs(pwmotor)) {
Socrates 1:95dd5c626960 180 deliver=deliver*1.01;
Socrates 1:95dd5c626960 181 }
Socrates 1:95dd5c626960 182 if (abs(deliver) >= abs(pwmotor)) {
Socrates 1:95dd5c626960 183 deliver=deliver*0.99;
Socrates 1:95dd5c626960 184 }
Socrates 1:95dd5c626960 185 motorplus.write(0.5+deliver);
Socrates 1:95dd5c626960 186 motormin.write(0.5-deliver);
Socrates 0:095ff84c3ee9 187 }