Ralph Lentink / Mbed 2 deprecated BO

hfdh

Dependencies:   Encoder MODSERIAL mbed

Fork of BO by Thomas Plaisier

main.cpp@2:9dc7b40286d1, 2014-06-12 (annotated)

Committer:
Socrates
Date:
Thu Jun 12 08:44:25 2014 +0000
Revision:
2:9dc7b40286d1
Parent:
1:95dd5c626960
Child:
3:123f3fd0daf6
Hij loopt het normale gedeelte niet. Calibratie werkt. Klokken zit er nu in, gaat er zo uit.

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