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@23:c97e206cf2a7, 2015-10-14 (annotated)

Committer:
Fernon
Date:
Wed Oct 14 14:55:21 2015 +0000
Revision:
23:c97e206cf2a7
Parent:
22:2e1713475f5f
Child:
24:711c7c388e57
Lijkt te werken, Hij blijft binnen de range van zoals ik heb gezegd dat hij moet blijven. Buiten de range pakt de out of range het over. Moet nog verder getest worden op fouten en motor 2 moet ook nog toegevoegd worden. Servo motor werkt ook goed.

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 22:2e1713475f5f 25 const double margin = 0.3;
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 22:2e1713475f5f 73 scope.set(2,emg_value);
Fernon 16:b0ec8e6a8ad4 74 scope.send();
Fernon 16:b0ec8e6a8ad4 75 }
Fernon 22:2e1713475f5f 76 void EMGsample()
Fernon 22:2e1713475f5f 77 {
Fernon 22:2e1713475f5f 78 // Lees het EMG signaal uit
Fernon 22:2e1713475f5f 79 //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
Fernon 22:2e1713475f5f 80 emg_value = PotMeter.read();
Fernon 22:2e1713475f5f 81 }
Fernon 22:2e1713475f5f 82
Fernon 22:2e1713475f5f 83
Fernon 22:2e1713475f5f 84
Fernon 0:5a5f417fa1b2 85 int main()
Fernon 0:5a5f417fa1b2 86 {
Fernon 11:a9a23042fc9e 87 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 88 pc.baud(115200);
Fernon 2:f0e9ffc5df09 89 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 90 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 91 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 23:c97e206cf2a7 92 sampleEMG.attach(EMGsample,0.5/Fs);
Fernon 23:c97e206cf2a7 93 PowerServo.period_ms(20);
Fernon 0:5a5f417fa1b2 94 while (true) {
Fernon 17:c5eea26e171d 95 Encoder.reset();
Fernon 11:a9a23042fc9e 96 if (Button == 0) {
Fernon 11:a9a23042fc9e 97 Excecute =! Excecute;
Fernon 17:c5eea26e171d 98 }
Fernon 22:2e1713475f5f 99 while (Excecute ) { //Dit wordt gebruikt voor motor 1
Fernon 23:c97e206cf2a7 100 //pc.printf("Rotatie = %f \n", Rotatie);
Fernon 23:c97e206cf2a7 101 //pc.printf("Out of Range = %i \n", OutRange);
Fernon 23:c97e206cf2a7 102 if (Rotatie >= upperlimit-margin) {
Fernon 22:2e1713475f5f 103 Goal = upperlimit - margin;
Fernon 23:c97e206cf2a7 104 OutRange = true;
Fernon 22:2e1713475f5f 105 }
Fernon 23:c97e206cf2a7 106 if (Rotatie <= downlimit+margin) {
Fernon 22:2e1713475f5f 107 Goal = 0 + margin;
Fernon 23:c97e206cf2a7 108 OutRange = true;
Fernon 22:2e1713475f5f 109 }
Fernon 23:c97e206cf2a7 110 if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
Fernon 23:c97e206cf2a7 111 OutRange = false;
Fernon 22:2e1713475f5f 112 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 23:c97e206cf2a7 113 if (emg_value >= T2 ) {
Fernon 22:2e1713475f5f 114 Direction = 1;
Fernon 22:2e1713475f5f 115 v = 1;
Fernon 22:2e1713475f5f 116 }
Fernon 22:2e1713475f5f 117 if (emg_value >= T1 && emg_value <= T2) {
Fernon 22:2e1713475f5f 118 Direction = 0;
Fernon 22:2e1713475f5f 119 v = 1;
Fernon 23:c97e206cf2a7 120 }
Fernon 23:c97e206cf2a7 121 if (emg_value <= T1){
Fernon 23:c97e206cf2a7 122 Direction = 0;
Fernon 23:c97e206cf2a7 123 v = 0;
Fernon 23:c97e206cf2a7 124 }
Fernon 23:c97e206cf2a7 125 }
Fernon 23:c97e206cf2a7 126 }
Fernon 23:c97e206cf2a7 127 while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!!
Fernon 23:c97e206cf2a7 128 if (Rotatie >= upperlimit-margin) {
Fernon 23:c97e206cf2a7 129 Goal = upperlimit - margin;
Fernon 23:c97e206cf2a7 130 OutRange = true;
Fernon 23:c97e206cf2a7 131 }
Fernon 23:c97e206cf2a7 132 if (Rotatie <= downlimit+margin) {
Fernon 23:c97e206cf2a7 133 Goal = 0 + margin;
Fernon 23:c97e206cf2a7 134 OutRange = true;
Fernon 23:c97e206cf2a7 135 }
Fernon 23:c97e206cf2a7 136 if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan
Fernon 23:c97e206cf2a7 137 OutRange = false;
Fernon 23:c97e206cf2a7 138 // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden
Fernon 23:c97e206cf2a7 139 if (emg_value >= T2 ) {
Fernon 23:c97e206cf2a7 140 Direction = 0;
Fernon 23:c97e206cf2a7 141 v = 1;
Fernon 23:c97e206cf2a7 142 }
Fernon 23:c97e206cf2a7 143 if (emg_value >= T1 && emg_value <= T2) {
Fernon 23:c97e206cf2a7 144 Direction = 1;
Fernon 23:c97e206cf2a7 145 v = 1;
Fernon 22:2e1713475f5f 146 } else {
Fernon 22:2e1713475f5f 147 Direction = 0;
Fernon 22:2e1713475f5f 148 v = 0;
Fernon 22:2e1713475f5f 149 }
Fernon 18:0f7f57228901 150 }
Fernon 22:2e1713475f5f 151 }
Fernon 22:2e1713475f5f 152
Fernon 23:c97e206cf2a7 153 if (Button2 == 0) {
Fernon 23:c97e206cf2a7 154 PowerServo.write(0.27);
Fernon 23:c97e206cf2a7 155 wait (1);
Fernon 23:c97e206cf2a7 156 PowerServo.write(0.04);
Fernon 23:c97e206cf2a7 157 wait (1);
Fernon 23:c97e206cf2a7 158 Fire=Fire+1;
Fernon 23:c97e206cf2a7 159 }
Fernon 23:c97e206cf2a7 160 if (Fire == 3) {
Fernon 23:c97e206cf2a7 161 wait (1);
Fernon 23:c97e206cf2a7 162 Excecute = false;
Fernon 23:c97e206cf2a7 163 Home = true;
Fernon 23:c97e206cf2a7 164 }
Fernon 23:c97e206cf2a7 165
Fernon 23:c97e206cf2a7 166
Fernon 22:2e1713475f5f 167 while (Home) {
Fernon 23:c97e206cf2a7 168 OutRange = false;
Fernon 23:c97e206cf2a7 169 Goal = margin; // Het doel waar hij naar toe moet
Fernon 21:d0156eadcbfe 170 if (fabs(Error)<=0.0015) {
Fernon 21:d0156eadcbfe 171 timer.start();
Fernon 21:d0156eadcbfe 172 } else {
Fernon 21:d0156eadcbfe 173 timer.stop();
Fernon 21:d0156eadcbfe 174 timer.reset();
Fernon 16:b0ec8e6a8ad4 175 }
Fernon 22:2e1713475f5f 176 if (timer.read() >= 3) {
Fernon 21:d0156eadcbfe 177 Excecute=false;
Fernon 22:2e1713475f5f 178 Home = false;
Fernon 18:0f7f57228901 179 Errori = 0;
Fernon 18:0f7f57228901 180 Errord = 0;
Fernon 11:a9a23042fc9e 181 }
Fernon 8:a2b725b502d8 182 }
Fernon 0:5a5f417fa1b2 183 }
Fernon 22:2e1713475f5f 184 }