Fernon Eijkhoudt / Mbed 2 deprecated ROBOBIRDS_FINAL

EMG driven robot which shoots elastic bands

Dependencies:   QEI mbed

Fork of RoboBirdV1 by Fernon Eijkhoudt

main.cpp@24:711c7c388e57, 2015-10-14 (annotated)

Committer:
Fernon
Date:
Wed Oct 14 15:27:45 2015 +0000
Revision:
24:711c7c388e57
Parent:
23:c97e206cf2a7
Child:
25:230bd4e1f3ef
Motor 1 werkt nu perfect (er moet nog wel even gekeken worden naar maximale rotatie, hier heb ik nog een fout in zitten). Motor 2 moet nog de range bepaald worden. Vuurknop werkt gewoon prima;

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 23:c97e206cf2a7 8 PwmOut PowerServo(D3);
Fernon 22:2e1713475f5f 9 DigitalIn Button(PTC6);
Fernon 23:c97e206cf2a7 10 DigitalIn Button2(PTA4);
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 23:c97e206cf2a7 17 Ticker sampleEMG;
Fernon 22:2e1713475f5f 18 Timer timer;
Fernon 22:2e1713475f5f 19 HIDScope scope(3);
Fernon 0:5a5f417fa1b2 20
yc238 15:f7e2b20f4dca 21
Fernon 23:c97e206cf2a7 22 double emg_value;
Fernon 22:2e1713475f5f 23 const double twopi = 6.2831853071795;
Fernon 22:2e1713475f5f 24 int Pulses;
Fernon 24:711c7c388e57 25 const double margin = 0.4;
Fernon 22:2e1713475f5f 26 double Rotatie = 0; //aantal graden dat de motor is gedraaid
Fernon 22:2e1713475f5f 27 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 22:2e1713475f5f 28 double Error = 0;
Fernon 22:2e1713475f5f 29 double Errord = 0;
Fernon 22:2e1713475f5f 30 double Errori = 0;
Fernon 22:2e1713475f5f 31 double Errorp = 0;
Fernon 22:2e1713475f5f 32 const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
Fernon 22:2e1713475f5f 33 const double Kd = 10;
Fernon 22:2e1713475f5f 34 const double Ki = 0.2;
Fernon 22:2e1713475f5f 35 double v = 0; //snelheid van de motor (0-0.1
Fernon 22:2e1713475f5f 36 double upperlimit; //max aantal rotaties
Fernon 23:c97e206cf2a7 37 const double downlimit = 0.3;
Fernon 22:2e1713475f5f 38 bool Excecute = false;
Fernon 22:2e1713475f5f 39 bool Home = false;
Fernon 23:c97e206cf2a7 40 bool OutRange = 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 23:c97e206cf2a7 50 if (OutRange) {
Fernon 23:c97e206cf2a7 51 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 23:c97e206cf2a7 52 Errord = (Error-Errorp)/Fs;
Fernon 23:c97e206cf2a7 53 Errorp = Error;
Fernon 23:c97e206cf2a7 54 if (fabs(Error) <= 0.5) {
Fernon 23:c97e206cf2a7 55 Errori = Errori + Error*1/Fs;
Fernon 23:c97e206cf2a7 56 } else {
Fernon 23:c97e206cf2a7 57 Errori = 0;
Fernon 23:c97e206cf2a7 58 }
Fernon 23:c97e206cf2a7 59 if (Error>=0) {
Fernon 23:c97e206cf2a7 60 Direction=1;
Fernon 23:c97e206cf2a7 61 } else {
Fernon 23:c97e206cf2a7 62 Direction=0;
Fernon 23:c97e206cf2a7 63 }
Fernon 23:c97e206cf2a7 64 v=Kp*Error + Kd*Errord + Ki*Errori;
Fernon 22:2e1713475f5f 65 }
yc238 14:b9c925a8176a 66 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 67 }
yc238 15:f7e2b20f4dca 68 void Send()
yc238 15:f7e2b20f4dca 69 {
Fernon 22:2e1713475f5f 70 Pulses = Encoder.getPulses();
Fernon 22:2e1713475f5f 71 Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
yc238 15:f7e2b20f4dca 72 scope.set(0,Goal);
Fernon 24:711c7c388e57 73 scope.set(1,Rotatie);
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 0:5a5f417fa1b2 86 int main()
Fernon 0:5a5f417fa1b2 87 {
Fernon 11:a9a23042fc9e 88 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 89 pc.baud(115200);
Fernon 2:f0e9ffc5df09 90 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 91 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 92 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 23:c97e206cf2a7 93 sampleEMG.attach(EMGsample,0.5/Fs);
Fernon 23:c97e206cf2a7 94 PowerServo.period_ms(20);
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 23:c97e206cf2a7 101 //pc.printf("Rotatie = %f \n", Rotatie);
Fernon 23:c97e206cf2a7 102 //pc.printf("Out of Range = %i \n", OutRange);
Fernon 24:711c7c388e57 103 if (Rotatie >= upperlimit) {
Fernon 22:2e1713475f5f 104 Goal = upperlimit - margin;
Fernon 23:c97e206cf2a7 105 OutRange = true;
Fernon 22:2e1713475f5f 106 }
Fernon 24:711c7c388e57 107 if (Rotatie <= downlimit) {
Fernon 22:2e1713475f5f 108 Goal = 0 + margin;
Fernon 23:c97e206cf2a7 109 OutRange = true;
Fernon 22:2e1713475f5f 110 }
Fernon 24:711c7c388e57 111 if (Rotatie >= margin && Rotatie <= upperlimit - margin) {
Fernon 23:c97e206cf2a7 112 OutRange = false;
Fernon 24:711c7c388e57 113 }
Fernon 24:711c7c388e57 114 if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) {
Fernon 22:2e1713475f5f 115 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 23:c97e206cf2a7 116 if (emg_value >= T2 ) {
Fernon 22:2e1713475f5f 117 Direction = 1;
Fernon 22:2e1713475f5f 118 v = 1;
Fernon 22:2e1713475f5f 119 }
Fernon 22:2e1713475f5f 120 if (emg_value >= T1 && emg_value <= T2) {
Fernon 22:2e1713475f5f 121 Direction = 0;
Fernon 22:2e1713475f5f 122 v = 1;
Fernon 24:711c7c388e57 123 }
Fernon 24:711c7c388e57 124 if (emg_value <= T1) {
Fernon 23:c97e206cf2a7 125 Direction = 0;
Fernon 23:c97e206cf2a7 126 v = 0;
Fernon 23:c97e206cf2a7 127 }
Fernon 23:c97e206cf2a7 128 }
Fernon 23:c97e206cf2a7 129 }
Fernon 24:711c7c388e57 130
Fernon 23:c97e206cf2a7 131 while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!!
Fernon 23:c97e206cf2a7 132 if (Rotatie >= upperlimit-margin) {
Fernon 23:c97e206cf2a7 133 Goal = upperlimit - margin;
Fernon 23:c97e206cf2a7 134 OutRange = true;
Fernon 23:c97e206cf2a7 135 }
Fernon 23:c97e206cf2a7 136 if (Rotatie <= downlimit+margin) {
Fernon 23:c97e206cf2a7 137 Goal = 0 + margin;
Fernon 23:c97e206cf2a7 138 OutRange = true;
Fernon 23:c97e206cf2a7 139 }
Fernon 23:c97e206cf2a7 140 if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
Fernon 23:c97e206cf2a7 141 OutRange = false;
Fernon 23:c97e206cf2a7 142 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 23:c97e206cf2a7 143 if (emg_value >= T2 ) {
Fernon 23:c97e206cf2a7 144 Direction = 0;
Fernon 23:c97e206cf2a7 145 v = 1;
Fernon 23:c97e206cf2a7 146 }
Fernon 23:c97e206cf2a7 147 if (emg_value >= T1 && emg_value <= T2) {
Fernon 23:c97e206cf2a7 148 Direction = 1;
Fernon 23:c97e206cf2a7 149 v = 1;
Fernon 22:2e1713475f5f 150 } else {
Fernon 22:2e1713475f5f 151 Direction = 0;
Fernon 22:2e1713475f5f 152 v = 0;
Fernon 22:2e1713475f5f 153 }
Fernon 18:0f7f57228901 154 }
Fernon 22:2e1713475f5f 155 }
Fernon 22:2e1713475f5f 156
Fernon 23:c97e206cf2a7 157 if (Button2 == 0) {
Fernon 23:c97e206cf2a7 158 PowerServo.write(0.27);
Fernon 23:c97e206cf2a7 159 wait (1);
Fernon 23:c97e206cf2a7 160 PowerServo.write(0.04);
Fernon 23:c97e206cf2a7 161 wait (1);
Fernon 23:c97e206cf2a7 162 Fire=Fire+1;
Fernon 23:c97e206cf2a7 163 }
Fernon 23:c97e206cf2a7 164 if (Fire == 3) {
Fernon 23:c97e206cf2a7 165 wait (1);
Fernon 23:c97e206cf2a7 166 Excecute = false;
Fernon 23:c97e206cf2a7 167 Home = true;
Fernon 23:c97e206cf2a7 168 }
Fernon 23:c97e206cf2a7 169
Fernon 23:c97e206cf2a7 170
Fernon 22:2e1713475f5f 171 while (Home) {
Fernon 23:c97e206cf2a7 172 OutRange = false;
Fernon 23:c97e206cf2a7 173 Goal = margin; // Het doel waar hij naar toe moet
Fernon 21:d0156eadcbfe 174 if (fabs(Error)<=0.0015) {
Fernon 21:d0156eadcbfe 175 timer.start();
Fernon 21:d0156eadcbfe 176 } else {
Fernon 21:d0156eadcbfe 177 timer.stop();
Fernon 21:d0156eadcbfe 178 timer.reset();
Fernon 16:b0ec8e6a8ad4 179 }
Fernon 22:2e1713475f5f 180 if (timer.read() >= 3) {
Fernon 21:d0156eadcbfe 181 Excecute=false;
Fernon 22:2e1713475f5f 182 Home = false;
Fernon 18:0f7f57228901 183 Errori = 0;
Fernon 18:0f7f57228901 184 Errord = 0;
Fernon 11:a9a23042fc9e 185 }
Fernon 8:a2b725b502d8 186 }
Fernon 0:5a5f417fa1b2 187 }
Fernon 22:2e1713475f5f 188 }