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@25:230bd4e1f3ef, 2015-10-15 (annotated)

Committer:
Fernon
Date:
Thu Oct 15 14:24:49 2015 +0000
Revision:
25:230bd4e1f3ef
Parent:
24:711c7c388e57
Child:
26:5b6c577fb3c1
2e motor erbij gevoegd

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 25:230bd4e1f3ef 6
Fernon 25:230bd4e1f3ef 7
Fernon 25:230bd4e1f3ef 8 // Motor 1 & 2
Fernon 22:2e1713475f5f 9 DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down
Fernon 22:2e1713475f5f 10 PwmOut PowerMotor(D5); //van 0 tot 1
Fernon 25:230bd4e1f3ef 11 QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder
Fernon 25:230bd4e1f3ef 12 DigitalOut Direction2(D6);
Fernon 25:230bd4e1f3ef 13 PwmOut PowerMotor2(D7);
Fernon 25:230bd4e1f3ef 14 QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
Fernon 23:c97e206cf2a7 15 PwmOut PowerServo(D3);
Fernon 25:230bd4e1f3ef 16
Fernon 25:230bd4e1f3ef 17 // Buttons & EMG (PotMeter)
Fernon 22:2e1713475f5f 18 DigitalIn Button(PTC6);
Fernon 23:c97e206cf2a7 19 DigitalIn Button2(PTA4);
Fernon 22:2e1713475f5f 20 AnalogIn PotMeter(A1);
Fernon 25:230bd4e1f3ef 21 AnalogIn PotMeter2(A2);
Fernon 25:230bd4e1f3ef 22 //AnalogIn EMG(A0);
Fernon 25:230bd4e1f3ef 23 //AnalogIn Emg(A1);
Fernon 25:230bd4e1f3ef 24
Fernon 25:230bd4e1f3ef 25 // Tickers & timers
Fernon 22:2e1713475f5f 26 Ticker MotorWrite;
Fernon 22:2e1713475f5f 27 Ticker Sender;
Fernon 23:c97e206cf2a7 28 Ticker sampleEMG;
Fernon 22:2e1713475f5f 29 Timer timer;
Fernon 25:230bd4e1f3ef 30
Fernon 25:230bd4e1f3ef 31 // Debugging
Fernon 25:230bd4e1f3ef 32 Serial pc(USBTX, USBRX);
Fernon 22:2e1713475f5f 33 HIDScope scope(3);
Fernon 0:5a5f417fa1b2 34
yc238 15:f7e2b20f4dca 35
Fernon 25:230bd4e1f3ef 36
Fernon 25:230bd4e1f3ef 37 // Waardes
Fernon 25:230bd4e1f3ef 38 const double twopi = 6.2831853071795;
Fernon 25:230bd4e1f3ef 39 const double Fs=100;
Fernon 25:230bd4e1f3ef 40 int Fired = 0;
Fernon 25:230bd4e1f3ef 41
Fernon 25:230bd4e1f3ef 42 // EMG
Fernon 23:c97e206cf2a7 43 double emg_value;
Fernon 25:230bd4e1f3ef 44 double emg_value2;
Fernon 25:230bd4e1f3ef 45 const double T1 = 0.33333; // Treshold 1
Fernon 25:230bd4e1f3ef 46 const double T2 = 0.66666; // Treshold 2
Fernon 25:230bd4e1f3ef 47
Fernon 25:230bd4e1f3ef 48 // Motor 1 (Translatie)
Fernon 25:230bd4e1f3ef 49 double n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog)
Fernon 22:2e1713475f5f 50 int Pulses;
Fernon 22:2e1713475f5f 51 double Rotatie = 0; //aantal graden dat de motor is gedraaid
Fernon 25:230bd4e1f3ef 52 double Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 22:2e1713475f5f 53 double Error = 0;
Fernon 22:2e1713475f5f 54 double Errord = 0;
Fernon 22:2e1713475f5f 55 double Errori = 0;
Fernon 22:2e1713475f5f 56 double Errorp = 0;
Fernon 22:2e1713475f5f 57 const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
Fernon 22:2e1713475f5f 58 const double Kd = 10;
Fernon 22:2e1713475f5f 59 const double Ki = 0.2;
Fernon 25:230bd4e1f3ef 60 double v = 0; //snelheid van de motor (0-0.1)?
Fernon 25:230bd4e1f3ef 61 double upperlimit; //max aantal rotaties omhoog
Fernon 25:230bd4e1f3ef 62 const double downlimit = 0.4;
Fernon 25:230bd4e1f3ef 63 const double margin = 0.4;
Fernon 25:230bd4e1f3ef 64 bool OutRange = false;
Fernon 25:230bd4e1f3ef 65
Fernon 25:230bd4e1f3ef 66 // Motor 2 (Rotatie)
Fernon 25:230bd4e1f3ef 67 double n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie)
Fernon 25:230bd4e1f3ef 68
Fernon 25:230bd4e1f3ef 69 int Pulses2;
Fernon 25:230bd4e1f3ef 70 double Rotatie2 = 0;
Fernon 25:230bd4e1f3ef 71 double Goal2;
Fernon 25:230bd4e1f3ef 72 double Error2 = 0;
Fernon 25:230bd4e1f3ef 73 double Errord2 = 0;
Fernon 25:230bd4e1f3ef 74 double Errori2 = 0;
Fernon 25:230bd4e1f3ef 75 double Errorp2 = 0;
Fernon 25:230bd4e1f3ef 76 const double Kp2 = 0.2;
Fernon 25:230bd4e1f3ef 77 const double Kd2 = 10;
Fernon 25:230bd4e1f3ef 78 const double Ki2 = 0.2;
Fernon 25:230bd4e1f3ef 79 double v2 = 0;
Fernon 25:230bd4e1f3ef 80 double turnlimit; // max aantal rotaties voor roteren hele robot
Fernon 25:230bd4e1f3ef 81 // Margin 2 is in ons geval 0
Fernon 25:230bd4e1f3ef 82 bool OutRange2 = false;
Fernon 25:230bd4e1f3ef 83
Fernon 25:230bd4e1f3ef 84 // Activatie tussen het schietgedeelte en terugkeergedeelte
Fernon 22:2e1713475f5f 85 bool Excecute = false;
Fernon 22:2e1713475f5f 86 bool Home = false;
Fernon 25:230bd4e1f3ef 87
Fernon 10:e210675cbe71 88
Fernon 25:230bd4e1f3ef 89
Fernon 25:230bd4e1f3ef 90 // Voids voor berekeningen in het hoofdprogramma
Fernon 2:f0e9ffc5df09 91
yc238 14:b9c925a8176a 92 void MotorSet()
Fernon 2:f0e9ffc5df09 93 {
Fernon 23:c97e206cf2a7 94 if (OutRange) {
Fernon 23:c97e206cf2a7 95 Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal
Fernon 23:c97e206cf2a7 96 Errord = (Error-Errorp)/Fs;
Fernon 23:c97e206cf2a7 97 Errorp = Error;
Fernon 25:230bd4e1f3ef 98 if (fabs(Error) <= 0.1) {
Fernon 23:c97e206cf2a7 99 Errori = Errori + Error*1/Fs;
Fernon 23:c97e206cf2a7 100 } else {
Fernon 23:c97e206cf2a7 101 Errori = 0;
Fernon 23:c97e206cf2a7 102 }
Fernon 23:c97e206cf2a7 103 if (Error>=0) {
Fernon 23:c97e206cf2a7 104 Direction=1;
Fernon 23:c97e206cf2a7 105 } else {
Fernon 23:c97e206cf2a7 106 Direction=0;
Fernon 23:c97e206cf2a7 107 }
Fernon 23:c97e206cf2a7 108 v=Kp*Error + Kd*Errord + Ki*Errori;
Fernon 22:2e1713475f5f 109 }
yc238 14:b9c925a8176a 110 PowerMotor.write(fabs(v));
Fernon 25:230bd4e1f3ef 111
Fernon 25:230bd4e1f3ef 112 if (OutRange2) {
Fernon 25:230bd4e1f3ef 113 Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
Fernon 25:230bd4e1f3ef 114 Errord2 = (Error2-Errorp2)/Fs;
Fernon 25:230bd4e1f3ef 115 Errorp2 = Error2;
Fernon 25:230bd4e1f3ef 116 if (fabs(Error2) <= 0.5) {
Fernon 25:230bd4e1f3ef 117 Errori2 = Errori2 + Error2*1/Fs;
Fernon 25:230bd4e1f3ef 118 } else {
Fernon 25:230bd4e1f3ef 119 Errori2 = 0;
Fernon 25:230bd4e1f3ef 120 }
Fernon 25:230bd4e1f3ef 121 if (Error2>=0) {
Fernon 25:230bd4e1f3ef 122 Direction=1;
Fernon 25:230bd4e1f3ef 123 } else {
Fernon 25:230bd4e1f3ef 124 Direction=0;
Fernon 25:230bd4e1f3ef 125 }
Fernon 25:230bd4e1f3ef 126 v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
Fernon 25:230bd4e1f3ef 127 }
Fernon 25:230bd4e1f3ef 128 PowerMotor2.write(fabs(v2));
Fernon 2:f0e9ffc5df09 129 }
yc238 15:f7e2b20f4dca 130 void Send()
yc238 15:f7e2b20f4dca 131 {
Fernon 22:2e1713475f5f 132 Pulses = Encoder.getPulses();
Fernon 22:2e1713475f5f 133 Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien
Fernon 25:230bd4e1f3ef 134 Pulses2 = Encoder2.getPulses();
Fernon 25:230bd4e1f3ef 135 Rotatie2 = (Pulses*twopi)/4200;
yc238 15:f7e2b20f4dca 136 scope.set(0,Goal);
Fernon 24:711c7c388e57 137 scope.set(1,Rotatie);
Fernon 22:2e1713475f5f 138 scope.set(2,emg_value);
Fernon 16:b0ec8e6a8ad4 139 scope.send();
Fernon 16:b0ec8e6a8ad4 140 }
Fernon 22:2e1713475f5f 141 void EMGsample()
Fernon 22:2e1713475f5f 142 {
Fernon 22:2e1713475f5f 143 // Lees het EMG signaal uit
Fernon 22:2e1713475f5f 144 //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
Fernon 22:2e1713475f5f 145 emg_value = PotMeter.read();
Fernon 25:230bd4e1f3ef 146 emg_value2 = PotMeter2.read();
Fernon 22:2e1713475f5f 147 }
Fernon 22:2e1713475f5f 148
Fernon 25:230bd4e1f3ef 149 void Fire ()
Fernon 25:230bd4e1f3ef 150 {
Fernon 25:230bd4e1f3ef 151 PowerServo.write(0.27);
Fernon 25:230bd4e1f3ef 152 wait (1);
Fernon 25:230bd4e1f3ef 153 PowerServo.write(0.04);
Fernon 25:230bd4e1f3ef 154 wait (1);
Fernon 25:230bd4e1f3ef 155 Fired=Fired+1;
Fernon 25:230bd4e1f3ef 156 if (Fired >= 3) {
Fernon 25:230bd4e1f3ef 157 wait (1);
Fernon 25:230bd4e1f3ef 158 Excecute = false;
Fernon 25:230bd4e1f3ef 159 Home = true;
Fernon 25:230bd4e1f3ef 160 Fired = 0;
Fernon 25:230bd4e1f3ef 161 }
Fernon 25:230bd4e1f3ef 162 }
Fernon 25:230bd4e1f3ef 163
Fernon 25:230bd4e1f3ef 164
Fernon 25:230bd4e1f3ef 165 // Calibratie moet nog worden uitgevoerd!!!!
Fernon 22:2e1713475f5f 166
Fernon 22:2e1713475f5f 167
Fernon 0:5a5f417fa1b2 168 int main()
Fernon 0:5a5f417fa1b2 169 {
Fernon 25:230bd4e1f3ef 170 upperlimit = n1*twopi;
Fernon 25:230bd4e1f3ef 171 turnlimit = n2*twopi;
Fernon 2:f0e9ffc5df09 172 pc.baud(115200);
Fernon 2:f0e9ffc5df09 173 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 174 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 175 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 23:c97e206cf2a7 176 sampleEMG.attach(EMGsample,0.5/Fs);
Fernon 23:c97e206cf2a7 177 PowerServo.period_ms(20);
Fernon 0:5a5f417fa1b2 178 while (true) {
Fernon 17:c5eea26e171d 179 Encoder.reset();
Fernon 25:230bd4e1f3ef 180 Encoder2.reset();
Fernon 11:a9a23042fc9e 181 if (Button == 0) {
Fernon 11:a9a23042fc9e 182 Excecute =! Excecute;
Fernon 17:c5eea26e171d 183 }
Fernon 25:230bd4e1f3ef 184
Fernon 25:230bd4e1f3ef 185 while (Excecute ) {
Fernon 25:230bd4e1f3ef 186 // Eerst wordt motor 1 aangestuurd
Fernon 25:230bd4e1f3ef 187 if (Rotatie >= upperlimit) { //Als hij buiten bereik is
Fernon 22:2e1713475f5f 188 Goal = upperlimit - margin;
Fernon 23:c97e206cf2a7 189 OutRange = true;
Fernon 22:2e1713475f5f 190 }
Fernon 25:230bd4e1f3ef 191 if (Rotatie <= downlimit) { //Als hij buiten bereik is
Fernon 22:2e1713475f5f 192 Goal = 0 + margin;
Fernon 23:c97e206cf2a7 193 OutRange = true;
Fernon 22:2e1713475f5f 194 }
Fernon 25:230bd4e1f3ef 195 if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 23:c97e206cf2a7 196 OutRange = false;
Fernon 24:711c7c388e57 197 }
Fernon 25:230bd4e1f3ef 198 if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
Fernon 23:c97e206cf2a7 199 if (emg_value >= T2 ) {
Fernon 22:2e1713475f5f 200 Direction = 1;
Fernon 22:2e1713475f5f 201 v = 1;
Fernon 22:2e1713475f5f 202 }
Fernon 22:2e1713475f5f 203 if (emg_value >= T1 && emg_value <= T2) {
Fernon 22:2e1713475f5f 204 Direction = 0;
Fernon 22:2e1713475f5f 205 v = 1;
Fernon 24:711c7c388e57 206 }
Fernon 24:711c7c388e57 207 if (emg_value <= T1) {
Fernon 23:c97e206cf2a7 208 Direction = 0;
Fernon 23:c97e206cf2a7 209 v = 0;
Fernon 23:c97e206cf2a7 210 }
Fernon 23:c97e206cf2a7 211 }
Fernon 24:711c7c388e57 212
Fernon 25:230bd4e1f3ef 213 // Vanaf hier wordt motor 2 aangestuurd
Fernon 25:230bd4e1f3ef 214 if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
Fernon 25:230bd4e1f3ef 215 Goal2 = turnlimit;
Fernon 25:230bd4e1f3ef 216 OutRange2 = true;
Fernon 25:230bd4e1f3ef 217 }
Fernon 25:230bd4e1f3ef 218 if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
Fernon 25:230bd4e1f3ef 219 Goal2 = -turnlimit;
Fernon 25:230bd4e1f3ef 220 OutRange2 = true;
Fernon 23:c97e206cf2a7 221 }
Fernon 25:230bd4e1f3ef 222 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 25:230bd4e1f3ef 223 OutRange2 = false;
Fernon 23:c97e206cf2a7 224 }
Fernon 25:230bd4e1f3ef 225 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
Fernon 25:230bd4e1f3ef 226
Fernon 25:230bd4e1f3ef 227 if (emg_value2 >= T2 ) {
Fernon 25:230bd4e1f3ef 228 Direction = 1;
Fernon 25:230bd4e1f3ef 229 v = 1;
Fernon 25:230bd4e1f3ef 230 }
Fernon 25:230bd4e1f3ef 231 if (emg_value2 >= T1 && emg_value2 <= T2) {
Fernon 23:c97e206cf2a7 232 Direction = 0;
Fernon 23:c97e206cf2a7 233 v = 1;
Fernon 23:c97e206cf2a7 234 }
Fernon 25:230bd4e1f3ef 235 if (emg_value2 <= T1) {
Fernon 22:2e1713475f5f 236 Direction = 0;
Fernon 22:2e1713475f5f 237 v = 0;
Fernon 22:2e1713475f5f 238 }
Fernon 18:0f7f57228901 239 }
Fernon 25:230bd4e1f3ef 240 if (Button2 == 0) { //Afvuren van de RBG
Fernon 25:230bd4e1f3ef 241 Fire();
Fernon 25:230bd4e1f3ef 242 }
Fernon 22:2e1713475f5f 243 }
Fernon 22:2e1713475f5f 244
Fernon 25:230bd4e1f3ef 245 while (Home) { //Terugkeren naar vaste positie
Fernon 25:230bd4e1f3ef 246 OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
Fernon 25:230bd4e1f3ef 247 Goal = 0;
Fernon 25:230bd4e1f3ef 248 Goal2 = 0;
Fernon 25:230bd4e1f3ef 249 if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) {
Fernon 21:d0156eadcbfe 250 timer.start();
Fernon 21:d0156eadcbfe 251 } else {
Fernon 21:d0156eadcbfe 252 timer.stop();
Fernon 21:d0156eadcbfe 253 timer.reset();
Fernon 16:b0ec8e6a8ad4 254 }
Fernon 22:2e1713475f5f 255 if (timer.read() >= 3) {
Fernon 22:2e1713475f5f 256 Home = false;
Fernon 18:0f7f57228901 257 Errori = 0;
Fernon 18:0f7f57228901 258 Errord = 0;
Fernon 25:230bd4e1f3ef 259 Errorp = 0;
Fernon 25:230bd4e1f3ef 260 Errori2 = 0;
Fernon 25:230bd4e1f3ef 261 Errord2 = 0;
Fernon 25:230bd4e1f3ef 262 Errorp2 = 0;
Fernon 25:230bd4e1f3ef 263 wait (10);
Fernon 25:230bd4e1f3ef 264 Excecute = true;
Fernon 11:a9a23042fc9e 265 }
Fernon 8:a2b725b502d8 266 }
Fernon 0:5a5f417fa1b2 267 }
Fernon 22:2e1713475f5f 268 }