Fernon Eijkhoudt / Mbed 2 deprecated RoboBirdV1

Alles in 1

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of RoboBird3 by Fernon Eijkhoudt

main.cpp@12:dcf90cafb2a5, 2015-09-30 (annotated)

Committer:
yc238
Date:
Wed Sep 30 19:25:46 2015 +0000
Revision:
12:dcf90cafb2a5
Parent:
11:a9a23042fc9e
Child:
13:c60942a1da8e
Fernon, doe je best ermee

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fernon 0:5a5f417fa1b2 1 #include "mbed.h"
Fernon 0:5a5f417fa1b2 2 #include "QEI.h"
Fernon 8:a2b725b502d8 3 #include "math.h"
yc238 12:dcf90cafb2a5 4 #include "HIDScope.h"
Fernon 0:5a5f417fa1b2 5
Fernon 1:bb11e38dda43 6 DigitalOut Direction(D4); //1 = CCW - 0 = CW
Fernon 1:bb11e38dda43 7 PwmOut PowerMotor(D5); //van 0 tot 1
Fernon 11:a9a23042fc9e 8 DigitalIn Button(D1);
Fernon 1:bb11e38dda43 9 AnalogIn PotMeter(A1);
Fernon 1:bb11e38dda43 10 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Fernon 1:bb11e38dda43 11 Serial pc(USBTX, USBRX);
Fernon 2:f0e9ffc5df09 12 Ticker Pot;
yc238 12:dcf90cafb2a5 13 HIDScope scope(2);
Fernon 0:5a5f417fa1b2 14
Fernon 10:e210675cbe71 15 double z=0; //initiele waarde potmeter
Fernon 8:a2b725b502d8 16 const double twopi = 6.2831853071795;
Fernon 8:a2b725b502d8 17 const double pi = twopi/2;
Fernon 8:a2b725b502d8 18 int Pulses;
Fernon 10:e210675cbe71 19 double Rotatie; //aantal graden dat de motor is gedraaid
Fernon 10:e210675cbe71 20 double Rotatieup; //aantal graden dat de motor is gedraaid in een bereik van n*pi
Fernon 11:a9a23042fc9e 21 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 8:a2b725b502d8 22 double Error = 0;
Fernon 10:e210675cbe71 23 double Errorv = 0;
Fernon 10:e210675cbe71 24 double Kp = 2; //Moet berekend worden aan de hand van Control concept slides
Fernon 11:a9a23042fc9e 25 double Kd = 0.1;
Fernon 10:e210675cbe71 26 double v = 0; //snelheid van de motor (0-0.1
Fernon 10:e210675cbe71 27 double upperlimit; //max aantal rotaties
Fernon 11:a9a23042fc9e 28 bool Excecute = false;
Fernon 10:e210675cbe71 29
Fernon 10:e210675cbe71 30 double n = 2;
Fernon 2:f0e9ffc5df09 31
Fernon 5:d47e6a96256b 32 void readpot()
Fernon 2:f0e9ffc5df09 33 {
Fernon 10:e210675cbe71 34 z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld
Fernon 2:f0e9ffc5df09 35 }
Fernon 0:5a5f417fa1b2 36
Fernon 0:5a5f417fa1b2 37 int main()
Fernon 0:5a5f417fa1b2 38 {
Fernon 11:a9a23042fc9e 39 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 40 pc.baud(115200);
Fernon 2:f0e9ffc5df09 41 PowerMotor.write(0);
Fernon 6:cf20f04dbab4 42 Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
Fernon 0:5a5f417fa1b2 43 while (true) {
Fernon 11:a9a23042fc9e 44 if (Button == 0) {
Fernon 11:a9a23042fc9e 45 Excecute =! Excecute;
yc238 12:dcf90cafb2a5 46 }
yc238 12:dcf90cafb2a5 47 while (Excecute ) {
yc238 12:dcf90cafb2a5 48 Pulses = Encoder.getPulses();
yc238 12:dcf90cafb2a5 49 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
yc238 12:dcf90cafb2a5 50 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
yc238 12:dcf90cafb2a5 51 pc.printf ("Potmeter = %f\n", z);
yc238 12:dcf90cafb2a5 52 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
yc238 12:dcf90cafb2a5 53 Goal = z*upperlimit; // Het doel waar hij naar toe moet
yc238 12:dcf90cafb2a5 54 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
yc238 12:dcf90cafb2a5 55 Errorv = 0-v;
yc238 12:dcf90cafb2a5 56 if (Error >= 0) { //Bepalen van de rotatie richting
yc238 12:dcf90cafb2a5 57 Direction =0;
yc238 12:dcf90cafb2a5 58 } else {
yc238 12:dcf90cafb2a5 59 Direction =1;
Fernon 11:a9a23042fc9e 60 }
yc238 12:dcf90cafb2a5 61 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
yc238 12:dcf90cafb2a5 62 v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor
yc238 12:dcf90cafb2a5 63 PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1).
yc238 12:dcf90cafb2a5 64 if (Error<=0.001 && Error>=-0.001) {
yc238 12:dcf90cafb2a5 65 PowerMotor.write(0);
yc238 12:dcf90cafb2a5 66 Excecute=false;
Fernon 11:a9a23042fc9e 67 }
yc238 12:dcf90cafb2a5 68 scope.set(0,Goal);
yc238 12:dcf90cafb2a5 69 scope.set(1,Rotatieup);
yc238 12:dcf90cafb2a5 70 scope.send();
Fernon 8:a2b725b502d8 71 }
Fernon 0:5a5f417fa1b2 72 }
yc238 12:dcf90cafb2a5 73 }