Fernon Eijkhoudt / Mbed 2 deprecated ROBOBIRDSV2

EMG driven robot which shoots elastic bands

Dependencies:   QEI mbed

Fork of ROBOBIRDS_FINAL by Fernon Eijkhoudt

main.cpp@27:9cca2ad74ec0, 2015-10-15 (annotated)

Committer:
Fernon
Date:
Thu Oct 15 16:03:40 2015 +0000
Revision:
27:9cca2ad74ec0
Parent:
26:5b6c577fb3c1
Child:
28:b7d01a55530f
Motor 1 is nu goed aan te sturen, maar zodra ik motor 2 toevoeg gaat hij heel raar doen. Terug naar home doet het 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 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 int Pulses2;
Fernon 25:230bd4e1f3ef 69 double Rotatie2 = 0;
Fernon 25:230bd4e1f3ef 70 double Goal2;
Fernon 25:230bd4e1f3ef 71 double Error2 = 0;
Fernon 25:230bd4e1f3ef 72 double Errord2 = 0;
Fernon 25:230bd4e1f3ef 73 double Errori2 = 0;
Fernon 25:230bd4e1f3ef 74 double Errorp2 = 0;
Fernon 25:230bd4e1f3ef 75 const double Kp2 = 0.2;
Fernon 25:230bd4e1f3ef 76 const double Kd2 = 10;
Fernon 25:230bd4e1f3ef 77 const double Ki2 = 0.2;
Fernon 25:230bd4e1f3ef 78 double v2 = 0;
Fernon 25:230bd4e1f3ef 79 double turnlimit; // max aantal rotaties voor roteren hele robot
Fernon 25:230bd4e1f3ef 80 // Margin 2 is in ons geval 0
Fernon 25:230bd4e1f3ef 81 bool OutRange2 = false;
Fernon 25:230bd4e1f3ef 82
Fernon 25:230bd4e1f3ef 83 // Activatie tussen het schietgedeelte en terugkeergedeelte
Fernon 22:2e1713475f5f 84 bool Excecute = false;
Fernon 22:2e1713475f5f 85 bool Home = false;
Fernon 25:230bd4e1f3ef 86
Fernon 10:e210675cbe71 87
Fernon 25:230bd4e1f3ef 88
Fernon 25:230bd4e1f3ef 89 // Voids voor berekeningen in het hoofdprogramma
Fernon 2:f0e9ffc5df09 90
yc238 14:b9c925a8176a 91 void MotorSet()
Fernon 2:f0e9ffc5df09 92 {
Fernon 27:9cca2ad74ec0 93 // Eerst motor 1 (translatie)
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 27:9cca2ad74ec0 98 if (fabs(Error) <= 0.5) {
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 27:9cca2ad74ec0 112 // Dan motor 2 (rotatie)
Fernon 27:9cca2ad74ec0 113 if (OutRange2) {
Fernon 27:9cca2ad74ec0 114 Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal
Fernon 27:9cca2ad74ec0 115 Errord2 = (Error2-Errorp2)/Fs;
Fernon 27:9cca2ad74ec0 116 Errorp2 = Error2;
Fernon 27:9cca2ad74ec0 117 if (fabs(Error2) <= 0.5) {
Fernon 27:9cca2ad74ec0 118 Errori2 = Errori2 + Error2*1/Fs;
Fernon 27:9cca2ad74ec0 119 } else {
Fernon 27:9cca2ad74ec0 120 Errori2 = 0;
Fernon 27:9cca2ad74ec0 121 }
Fernon 27:9cca2ad74ec0 122 if (Error2>=0) {
Fernon 27:9cca2ad74ec0 123 Direction=1;
Fernon 27:9cca2ad74ec0 124 } else {
Fernon 27:9cca2ad74ec0 125 Direction=0;
Fernon 27:9cca2ad74ec0 126 }
Fernon 27:9cca2ad74ec0 127 v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
Fernon 27:9cca2ad74ec0 128 }
Fernon 27:9cca2ad74ec0 129 PowerMotor2.write(fabs(v2));
Fernon 2:f0e9ffc5df09 130 }
yc238 15:f7e2b20f4dca 131 void Send()
yc238 15:f7e2b20f4dca 132 {
Fernon 22:2e1713475f5f 133 Pulses = Encoder.getPulses();
Fernon 27:9cca2ad74ec0 134 Rotatie = (Pulses*twopi)/4200;
Fernon 25:230bd4e1f3ef 135 Pulses2 = Encoder2.getPulses();
Fernon 26:5b6c577fb3c1 136 Rotatie2 = (Pulses2*twopi)/4200;
yc238 15:f7e2b20f4dca 137 scope.set(0,Goal);
Fernon 24:711c7c388e57 138 scope.set(1,Rotatie);
Fernon 22:2e1713475f5f 139 scope.set(2,emg_value);
Fernon 16:b0ec8e6a8ad4 140 scope.send();
Fernon 16:b0ec8e6a8ad4 141 }
Fernon 22:2e1713475f5f 142 void EMGsample()
Fernon 22:2e1713475f5f 143 {
Fernon 22:2e1713475f5f 144 // Lees het EMG signaal uit
Fernon 22:2e1713475f5f 145 //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
Fernon 22:2e1713475f5f 146 emg_value = PotMeter.read();
Fernon 25:230bd4e1f3ef 147 emg_value2 = PotMeter2.read();
Fernon 22:2e1713475f5f 148 }
Fernon 22:2e1713475f5f 149
Fernon 25:230bd4e1f3ef 150 void Fire ()
Fernon 25:230bd4e1f3ef 151 {
Fernon 25:230bd4e1f3ef 152 PowerServo.write(0.27);
Fernon 25:230bd4e1f3ef 153 wait (1);
Fernon 25:230bd4e1f3ef 154 PowerServo.write(0.04);
Fernon 25:230bd4e1f3ef 155 wait (1);
Fernon 25:230bd4e1f3ef 156 Fired=Fired+1;
Fernon 25:230bd4e1f3ef 157 if (Fired >= 3) {
Fernon 25:230bd4e1f3ef 158 wait (1);
Fernon 25:230bd4e1f3ef 159 Excecute = false;
Fernon 25:230bd4e1f3ef 160 Home = true;
Fernon 25:230bd4e1f3ef 161 Fired = 0;
Fernon 25:230bd4e1f3ef 162 }
Fernon 25:230bd4e1f3ef 163 }
Fernon 25:230bd4e1f3ef 164
Fernon 25:230bd4e1f3ef 165
Fernon 25:230bd4e1f3ef 166 // Calibratie moet nog worden uitgevoerd!!!!
Fernon 22:2e1713475f5f 167
Fernon 22:2e1713475f5f 168
Fernon 0:5a5f417fa1b2 169 int main()
Fernon 0:5a5f417fa1b2 170 {
Fernon 25:230bd4e1f3ef 171 upperlimit = n1*twopi;
Fernon 25:230bd4e1f3ef 172 turnlimit = n2*twopi;
Fernon 2:f0e9ffc5df09 173 pc.baud(115200);
Fernon 2:f0e9ffc5df09 174 PowerMotor.write(0);
Fernon 16:b0ec8e6a8ad4 175 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 176 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 23:c97e206cf2a7 177 sampleEMG.attach(EMGsample,0.5/Fs);
Fernon 23:c97e206cf2a7 178 PowerServo.period_ms(20);
Fernon 0:5a5f417fa1b2 179 while (true) {
Fernon 17:c5eea26e171d 180 Encoder.reset();
Fernon 25:230bd4e1f3ef 181 Encoder2.reset();
Fernon 11:a9a23042fc9e 182 if (Button == 0) {
Fernon 11:a9a23042fc9e 183 Excecute =! Excecute;
Fernon 17:c5eea26e171d 184 }
Fernon 25:230bd4e1f3ef 185 while (Excecute ) {
Fernon 25:230bd4e1f3ef 186 // Eerst wordt motor 1 aangestuurd
Fernon 26:5b6c577fb3c1 187 pc.printf("Rotatie = %f \n", Rotatie);
Fernon 27:9cca2ad74ec0 188 pc.printf("Rotatie2 = %f \n", Rotatie2);
Fernon 25:230bd4e1f3ef 189 if (Rotatie >= upperlimit) { //Als hij buiten bereik is
Fernon 27:9cca2ad74ec0 190 Goal = upperlimit - margin;
Fernon 23:c97e206cf2a7 191 OutRange = true;
Fernon 22:2e1713475f5f 192 }
Fernon 25:230bd4e1f3ef 193 if (Rotatie <= downlimit) { //Als hij buiten bereik is
Fernon 27:9cca2ad74ec0 194 Goal = 0 + margin;
Fernon 23:c97e206cf2a7 195 OutRange = true;
Fernon 22:2e1713475f5f 196 }
Fernon 25:230bd4e1f3ef 197 if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 23:c97e206cf2a7 198 OutRange = false;
Fernon 24:711c7c388e57 199 }
Fernon 25:230bd4e1f3ef 200 if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
Fernon 23:c97e206cf2a7 201 if (emg_value >= T2 ) {
Fernon 22:2e1713475f5f 202 Direction = 1;
Fernon 22:2e1713475f5f 203 v = 1;
Fernon 22:2e1713475f5f 204 }
Fernon 22:2e1713475f5f 205 if (emg_value >= T1 && emg_value <= T2) {
Fernon 22:2e1713475f5f 206 Direction = 0;
Fernon 22:2e1713475f5f 207 v = 1;
Fernon 24:711c7c388e57 208 }
Fernon 24:711c7c388e57 209 if (emg_value <= T1) {
Fernon 23:c97e206cf2a7 210 Direction = 0;
Fernon 23:c97e206cf2a7 211 v = 0;
Fernon 23:c97e206cf2a7 212 }
Fernon 23:c97e206cf2a7 213 }
Fernon 24:711c7c388e57 214
Fernon 26:5b6c577fb3c1 215 //// Vanaf hier wordt motor 2 aangestuurd
Fernon 26:5b6c577fb3c1 216 // if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
Fernon 26:5b6c577fb3c1 217 // Goal2 = turnlimit;
Fernon 26:5b6c577fb3c1 218 // OutRange2 = true;
Fernon 26:5b6c577fb3c1 219 // }
Fernon 26:5b6c577fb3c1 220 // if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
Fernon 26:5b6c577fb3c1 221 // Goal2 = -turnlimit;
Fernon 26:5b6c577fb3c1 222 // OutRange2 = true;
Fernon 26:5b6c577fb3c1 223 // }
Fernon 26:5b6c577fb3c1 224 // if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 26:5b6c577fb3c1 225 // OutRange2 = false;
Fernon 26:5b6c577fb3c1 226 // }
Fernon 26:5b6c577fb3c1 227 // if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
Fernon 26:5b6c577fb3c1 228 //
Fernon 26:5b6c577fb3c1 229 // if (emg_value2 >= T2 ) {
Fernon 26:5b6c577fb3c1 230 // Direction = 1;
Fernon 26:5b6c577fb3c1 231 // v = 1;
Fernon 26:5b6c577fb3c1 232 // }
Fernon 26:5b6c577fb3c1 233 // if (emg_value2 >= T1 && emg_value2 <= T2) {
Fernon 26:5b6c577fb3c1 234 // Direction = 0;
Fernon 26:5b6c577fb3c1 235 // v = 1;
Fernon 26:5b6c577fb3c1 236 // }
Fernon 26:5b6c577fb3c1 237 // if (emg_value2 <= T1) {
Fernon 26:5b6c577fb3c1 238 // Direction = 0;
Fernon 26:5b6c577fb3c1 239 // v = 0;
Fernon 26:5b6c577fb3c1 240 // }
Fernon 26:5b6c577fb3c1 241 // }
Fernon 27:9cca2ad74ec0 242 if (Button2 == 0) { //Afvuren van de RBG
Fernon 27:9cca2ad74ec0 243 Fire();
Fernon 27:9cca2ad74ec0 244 }
Fernon 22:2e1713475f5f 245 }
Fernon 22:2e1713475f5f 246
Fernon 25:230bd4e1f3ef 247 while (Home) { //Terugkeren naar vaste positie
Fernon 25:230bd4e1f3ef 248 OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
Fernon 25:230bd4e1f3ef 249 Goal = 0;
Fernon 25:230bd4e1f3ef 250 Goal2 = 0;
Fernon 25:230bd4e1f3ef 251 if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) {
Fernon 21:d0156eadcbfe 252 timer.start();
Fernon 21:d0156eadcbfe 253 } else {
Fernon 21:d0156eadcbfe 254 timer.stop();
Fernon 21:d0156eadcbfe 255 timer.reset();
Fernon 16:b0ec8e6a8ad4 256 }
Fernon 22:2e1713475f5f 257 if (timer.read() >= 3) {
Fernon 22:2e1713475f5f 258 Home = false;
Fernon 18:0f7f57228901 259 Errori = 0;
Fernon 18:0f7f57228901 260 Errord = 0;
Fernon 25:230bd4e1f3ef 261 Errorp = 0;
Fernon 25:230bd4e1f3ef 262 Errori2 = 0;
Fernon 25:230bd4e1f3ef 263 Errord2 = 0;
Fernon 25:230bd4e1f3ef 264 Errorp2 = 0;
Fernon 11:a9a23042fc9e 265 }
Fernon 8:a2b725b502d8 266 }
Fernon 0:5a5f417fa1b2 267 }
Fernon 22:2e1713475f5f 268 }