Fernon Eijkhoudt / Mbed 2 deprecated RoboBirdV1

Alles in 1

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of RoboBird3 by Fernon Eijkhoudt

main.cpp@22:2e1713475f5f, 2015-10-12 (annotated)

Committer:
Fernon
Date:
Mon Oct 12 18:23:05 2015 +0000
Revision:
22:2e1713475f5f
Parent:
21:d0156eadcbfe
Child:
23:c97e206cf2a7
First try on our real programme, not tested yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fernon 0:5a5f417fa1b2 1 #include "mbed.h"
Fernon 16:b0ec8e6a8ad4 2 #include "QEI.h"
Fernon 8:a2b725b502d8 3 #include "math.h"
yc238 12:dcf90cafb2a5 4 #include "HIDScope.h"
Fernon 0:5a5f417fa1b2 5
Fernon 22:2e1713475f5f 6 DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
Fernon 22:2e1713475f5f 7 PwmOut PowerMotor(D5); //van 0 tot 1
Fernon 22:2e1713475f5f 8 PwmOut PowerServo(D7);
Fernon 22:2e1713475f5f 9 DigitalIn Button(PTC6);
Fernon 22:2e1713475f5f 10 DigitalIn Button2(PTC4);
Fernon 22:2e1713475f5f 11 AnalogIn PotMeter(A1);
Fernon 22:2e1713475f5f 12 AnalogIn EMG(A0);
Fernon 22:2e1713475f5f 13 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Fernon 22:2e1713475f5f 14 Serial pc(USBTX, USBRX);
Fernon 22:2e1713475f5f 15 Ticker MotorWrite;
Fernon 22:2e1713475f5f 16 Ticker Sender;
Fernon 22:2e1713475f5f 17 Ticker EMG;
Fernon 22:2e1713475f5f 18 Timer timer;
Fernon 22:2e1713475f5f 19 HIDScope scope(3);
Fernon 0:5a5f417fa1b2 20
yc238 15:f7e2b20f4dca 21
Fernon 22:2e1713475f5f 22 double emg_value
Fernon 22:2e1713475f5f 23 const double twopi = 6.2831853071795;
Fernon 22:2e1713475f5f 24 int Pulses;
Fernon 22:2e1713475f5f 25 const double downlimit = 0.3;
Fernon 22:2e1713475f5f 26 const double margin = 0.3;
Fernon 22:2e1713475f5f 27 double Rotatie = 0; //aantal graden dat de motor is gedraaid
Fernon 22:2e1713475f5f 28 double Rotatieup = 0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
Fernon 22:2e1713475f5f 29 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 22:2e1713475f5f 30 double Error = 0;
Fernon 22:2e1713475f5f 31 double Errord = 0;
Fernon 22:2e1713475f5f 32 double Errori = 0;
Fernon 22:2e1713475f5f 33 double Errorp = 0;
Fernon 22:2e1713475f5f 34 const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
Fernon 22:2e1713475f5f 35 const double Kd = 10;
Fernon 22:2e1713475f5f 36 const double Ki = 0.2;
Fernon 22:2e1713475f5f 37 double v = 0; //snelheid van de motor (0-0.1
Fernon 22:2e1713475f5f 38 double upperlimit; //max aantal rotaties
Fernon 22:2e1713475f5f 39 bool Excecute = false;
Fernon 22:2e1713475f5f 40 bool Home = false;
Fernon 22:2e1713475f5f 41 const double Fs=100;
Fernon 22:2e1713475f5f 42 const double T1 = 0.33333; // Treshold 1
Fernon 22:2e1713475f5f 43 const double T2 = 0.66666; // Treshold 2
Fernon 22:2e1713475f5f 44 int Fire = 0;
Fernon 10:e210675cbe71 45
Fernon 22:2e1713475f5f 46 double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien
Fernon 2:f0e9ffc5df09 47
yc238 14:b9c925a8176a 48 void MotorSet()
Fernon 2:f0e9ffc5df09 49 {
Fernon 22:2e1713475f5f 50
Fernon 22:2e1713475f5f 51 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 22:2e1713475f5f 52 Errord = (Error-Errorp)/Fs;
Fernon 22:2e1713475f5f 53 Errorp = Error;
Fernon 22:2e1713475f5f 54 if (fabs(Error) <= 0.5) {
Fernon 22:2e1713475f5f 55 Errori = Errori + Error*1/Fs;
Fernon 22:2e1713475f5f 56 } else {
Fernon 22:2e1713475f5f 57 Errori = 0;
Fernon 22:2e1713475f5f 58 }
Fernon 18:0f7f57228901 59 if (Error>=0) {
yc238 14:b9c925a8176a 60 Direction=1;
yc238 14:b9c925a8176a 61 } else {
yc238 14:b9c925a8176a 62 Direction=0;
yc238 14:b9c925a8176a 63 }
Fernon 22:2e1713475f5f 64 v=Kp*Error + Kd*Errord + Ki*Errori;
yc238 14:b9c925a8176a 65 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 66 }
yc238 15:f7e2b20f4dca 67 void Send()
yc238 15:f7e2b20f4dca 68 {
Fernon 22:2e1713475f5f 69 Pulses = Encoder.getPulses();
Fernon 22:2e1713475f5f 70 Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
Fernon 22:2e1713475f5f 71 Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit
yc238 15:f7e2b20f4dca 72 scope.set(0,Goal);
yc238 15:f7e2b20f4dca 73 scope.set(1,Rotatieup);
Fernon 22:2e1713475f5f 74 scope.set(2,emg_value);
Fernon 16:b0ec8e6a8ad4 75 scope.send();
Fernon 16:b0ec8e6a8ad4 76 }
Fernon 22:2e1713475f5f 77 void EMGsample()
Fernon 22:2e1713475f5f 78 {
Fernon 22:2e1713475f5f 79 // Lees het EMG signaal uit
Fernon 22:2e1713475f5f 80 //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
Fernon 22:2e1713475f5f 81 emg_value = PotMeter.read();
Fernon 22:2e1713475f5f 82
Fernon 22:2e1713475f5f 83 }
Fernon 22:2e1713475f5f 84
Fernon 22:2e1713475f5f 85
Fernon 22:2e1713475f5f 86
Fernon 0:5a5f417fa1b2 87 int main()
Fernon 0:5a5f417fa1b2 88 {
Fernon 11:a9a23042fc9e 89 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 90 pc.baud(115200);
Fernon 2:f0e9ffc5df09 91 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 92 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 93 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 22:2e1713475f5f 94 EMG.attach(EMGsample,0.5/Fs);
Fernon 0:5a5f417fa1b2 95 while (true) {
Fernon 17:c5eea26e171d 96 Encoder.reset();
Fernon 11:a9a23042fc9e 97 if (Button == 0) {
Fernon 11:a9a23042fc9e 98 Excecute =! Excecute;
Fernon 17:c5eea26e171d 99 }
Fernon 22:2e1713475f5f 100 while (Excecute ) { //Dit wordt gebruikt voor motor 1
Fernon 22:2e1713475f5f 101 if (Rotatie >= upperlimit) {
Fernon 22:2e1713475f5f 102 Goal = upperlimit - margin;
Fernon 22:2e1713475f5f 103 }
Fernon 22:2e1713475f5f 104 if (Rotatie <= downlimit) {
Fernon 22:2e1713475f5f 105 Goal = 0 + margin;
Fernon 22:2e1713475f5f 106 }
Fernon 22:2e1713475f5f 107
Fernon 22:2e1713475f5f 108 else {
Fernon 22:2e1713475f5f 109 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 22:2e1713475f5f 110 if (emg_value => T2 ) {
Fernon 22:2e1713475f5f 111 Direction = 1;
Fernon 22:2e1713475f5f 112 v = 1;
Fernon 22:2e1713475f5f 113 }
Fernon 22:2e1713475f5f 114 if (emg_value >= T1 && emg_value <= T2) {
Fernon 22:2e1713475f5f 115 Direction = 0;
Fernon 22:2e1713475f5f 116 v = 1;
Fernon 22:2e1713475f5f 117 } else {
Fernon 22:2e1713475f5f 118 Direction = 0;
Fernon 22:2e1713475f5f 119 v = 0;
Fernon 22:2e1713475f5f 120 }
Fernon 18:0f7f57228901 121 }
Fernon 22:2e1713475f5f 122 while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden
Fernon 22:2e1713475f5f 123 OutofRange = true if
Fernon 22:2e1713475f5f 124 if (Rotatie >= upperlimit) {
Fernon 22:2e1713475f5f 125 Goal = upperlimit - margin;
Fernon 22:2e1713475f5f 126 }
Fernon 22:2e1713475f5f 127 if (Rotatie <= downlimit) {
Fernon 22:2e1713475f5f 128 Goal = 0 + margin;
Fernon 22:2e1713475f5f 129 }
Fernon 22:2e1713475f5f 130
Fernon 22:2e1713475f5f 131 else {
Fernon 22:2e1713475f5f 132 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 22:2e1713475f5f 133 if (emg_value => T2 ) {
Fernon 22:2e1713475f5f 134 Direction = 1;
Fernon 22:2e1713475f5f 135 v = 1;
Fernon 22:2e1713475f5f 136 }
Fernon 22:2e1713475f5f 137 if (emg_value >= T1 && emg_value <= T2) {
Fernon 22:2e1713475f5f 138 Direction = 0;
Fernon 22:2e1713475f5f 139 v = 1;
Fernon 22:2e1713475f5f 140 } else {
Fernon 22:2e1713475f5f 141 Direction = 0;
Fernon 22:2e1713475f5f 142 v = 0;
Fernon 22:2e1713475f5f 143 }
Fernon 22:2e1713475f5f 144 }
Fernon 22:2e1713475f5f 145 }
Fernon 22:2e1713475f5f 146 if (Button2 = 0) {
Fernon 22:2e1713475f5f 147 // servo motor activation --> make a function of it
Fernon 22:2e1713475f5f 148 Fire=Fire+1;
Fernon 22:2e1713475f5f 149 }
Fernon 22:2e1713475f5f 150 if (Fire == 3) {
Fernon 22:2e1713475f5f 151 wait (1);
Fernon 22:2e1713475f5f 152 Excecute = false;
Fernon 22:2e1713475f5f 153 Home = true;
Fernon 22:2e1713475f5f 154 }
Fernon 22:2e1713475f5f 155 }
Fernon 22:2e1713475f5f 156
Fernon 22:2e1713475f5f 157 while (Home) {
Fernon 22:2e1713475f5f 158 Goal = 0.3; // Het doel waar hij naar toe moet
Fernon 21:d0156eadcbfe 159 if (fabs(Error)<=0.0015) {
Fernon 21:d0156eadcbfe 160 timer.start();
Fernon 21:d0156eadcbfe 161 } else {
Fernon 21:d0156eadcbfe 162 timer.stop();
Fernon 21:d0156eadcbfe 163 timer.reset();
Fernon 16:b0ec8e6a8ad4 164 }
Fernon 22:2e1713475f5f 165 if (timer.read() >= 3) {
Fernon 21:d0156eadcbfe 166 Excecute=false;
Fernon 22:2e1713475f5f 167 Home = false;
Fernon 18:0f7f57228901 168 Errori = 0;
Fernon 18:0f7f57228901 169 Errord = 0;
Fernon 11:a9a23042fc9e 170 }
Fernon 8:a2b725b502d8 171 }
Fernon 0:5a5f417fa1b2 172 }
Fernon 22:2e1713475f5f 173 }