Fernon Eijkhoudt / Mbed 2 deprecated RoboBird3

almost perfect

Dependencies:   Encoder HIDScope MODSERIAL QEI mbed

Fork of Robobird2 by Fernon Eijkhoudt

main.cpp@29:7653adbbb101, 2015-10-16 (annotated)

Committer:
Fernon
Date:
Fri Oct 16 10:35:24 2015 +0000
Revision:
29:7653adbbb101
Parent:
28:b7d01a55530f 
Potmeter doesn't work

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 28:b7d01a55530f 12 DigitalOut Direction2(D7);
Fernon 28:b7d01a55530f 13 PwmOut PowerMotor2(D6);
Fernon 29:7653adbbb101 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 28:b7d01a55530f 20 AnalogIn PotMeter(A0);
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 29:7653adbbb101 33 HIDScope scope(6);
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 28:b7d01a55530f 79 double turnlimit = 0.4; // 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 29:7653adbbb101 123 Direction2 = 0;
Fernon 27:9cca2ad74ec0 124 } else {
Fernon 29:7653adbbb101 125 Direction2 = 1;
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 29:7653adbbb101 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 29:7653adbbb101 140 scope.set(3,Goal2);
Fernon 29:7653adbbb101 141 scope.set(4,Rotatie2);
Fernon 29:7653adbbb101 142 scope.set(5,emg_value2);
Fernon 16:b0ec8e6a8ad4 143 scope.send();
Fernon 16:b0ec8e6a8ad4 144 }
Fernon 22:2e1713475f5f 145 void EMGsample()
Fernon 22:2e1713475f5f 146 {
Fernon 22:2e1713475f5f 147 // Lees het EMG signaal uit
Fernon 22:2e1713475f5f 148 //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben
Fernon 22:2e1713475f5f 149 emg_value = PotMeter.read();
Fernon 25:230bd4e1f3ef 150 emg_value2 = PotMeter2.read();
Fernon 22:2e1713475f5f 151 }
Fernon 22:2e1713475f5f 152
Fernon 25:230bd4e1f3ef 153 void Fire ()
Fernon 25:230bd4e1f3ef 154 {
Fernon 25:230bd4e1f3ef 155 PowerServo.write(0.27);
Fernon 25:230bd4e1f3ef 156 wait (1);
Fernon 25:230bd4e1f3ef 157 PowerServo.write(0.04);
Fernon 25:230bd4e1f3ef 158 wait (1);
Fernon 25:230bd4e1f3ef 159 Fired=Fired+1;
Fernon 25:230bd4e1f3ef 160 if (Fired >= 3) {
Fernon 25:230bd4e1f3ef 161 wait (1);
Fernon 25:230bd4e1f3ef 162 Excecute = false;
Fernon 25:230bd4e1f3ef 163 Home = true;
Fernon 25:230bd4e1f3ef 164 Fired = 0;
Fernon 25:230bd4e1f3ef 165 }
Fernon 25:230bd4e1f3ef 166 }
Fernon 25:230bd4e1f3ef 167
Fernon 25:230bd4e1f3ef 168
Fernon 25:230bd4e1f3ef 169 // Calibratie moet nog worden uitgevoerd!!!!
Fernon 22:2e1713475f5f 170
Fernon 22:2e1713475f5f 171
Fernon 0:5a5f417fa1b2 172 int main()
Fernon 0:5a5f417fa1b2 173 {
Fernon 25:230bd4e1f3ef 174 upperlimit = n1*twopi;
Fernon 25:230bd4e1f3ef 175 turnlimit = n2*twopi;
Fernon 2:f0e9ffc5df09 176 pc.baud(115200);
Fernon 2:f0e9ffc5df09 177 PowerMotor.write(0);
Fernon 28:b7d01a55530f 178 PowerMotor2.write(0);
Fernon 16:b0ec8e6a8ad4 179 Sender.attach(Send,0.5/Fs);
yc238 15:f7e2b20f4dca 180 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde
Fernon 23:c97e206cf2a7 181 sampleEMG.attach(EMGsample,0.5/Fs);
Fernon 23:c97e206cf2a7 182 PowerServo.period_ms(20);
Fernon 0:5a5f417fa1b2 183 while (true) {
Fernon 17:c5eea26e171d 184 Encoder.reset();
Fernon 29:7653adbbb101 185 Encoder2.reset();
Fernon 11:a9a23042fc9e 186 if (Button == 0) {
Fernon 11:a9a23042fc9e 187 Excecute =! Excecute;
Fernon 17:c5eea26e171d 188 }
Fernon 25:230bd4e1f3ef 189 while (Excecute ) {
Fernon 25:230bd4e1f3ef 190 // Eerst wordt motor 1 aangestuurd
Fernon 26:5b6c577fb3c1 191 pc.printf("Rotatie = %f \n", Rotatie);
Fernon 27:9cca2ad74ec0 192 pc.printf("Rotatie2 = %f \n", Rotatie2);
Fernon 25:230bd4e1f3ef 193 if (Rotatie >= upperlimit) { //Als hij buiten bereik is
Fernon 27:9cca2ad74ec0 194 Goal = upperlimit - margin;
Fernon 23:c97e206cf2a7 195 OutRange = true;
Fernon 22:2e1713475f5f 196 }
Fernon 25:230bd4e1f3ef 197 if (Rotatie <= downlimit) { //Als hij buiten bereik is
Fernon 27:9cca2ad74ec0 198 Goal = 0 + margin;
Fernon 23:c97e206cf2a7 199 OutRange = true;
Fernon 22:2e1713475f5f 200 }
Fernon 25:230bd4e1f3ef 201 if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 23:c97e206cf2a7 202 OutRange = false;
Fernon 24:711c7c388e57 203 }
Fernon 25:230bd4e1f3ef 204 if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing
Fernon 23:c97e206cf2a7 205 if (emg_value >= T2 ) {
Fernon 22:2e1713475f5f 206 Direction = 1;
Fernon 28:b7d01a55530f 207 v = 0.1;
Fernon 22:2e1713475f5f 208 }
Fernon 28:b7d01a55530f 209 if (emg_value > T1 && emg_value < T2) {
Fernon 22:2e1713475f5f 210 Direction = 0;
Fernon 28:b7d01a55530f 211 v = 0.1;
Fernon 24:711c7c388e57 212 }
Fernon 24:711c7c388e57 213 if (emg_value <= T1) {
Fernon 23:c97e206cf2a7 214 Direction = 0;
Fernon 23:c97e206cf2a7 215 v = 0;
Fernon 23:c97e206cf2a7 216 }
Fernon 23:c97e206cf2a7 217 }
Fernon 24:711c7c388e57 218
Fernon 28:b7d01a55530f 219 // Vanaf hier wordt motor 2 aangestuurd
Fernon 28:b7d01a55530f 220 if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is
Fernon 28:b7d01a55530f 221 Goal2 = turnlimit;
Fernon 28:b7d01a55530f 222 OutRange2 = true;
Fernon 28:b7d01a55530f 223 }
Fernon 28:b7d01a55530f 224 if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is
Fernon 28:b7d01a55530f 225 Goal2 = -turnlimit;
Fernon 28:b7d01a55530f 226 OutRange2 = true;
Fernon 28:b7d01a55530f 227 }
Fernon 28:b7d01a55530f 228 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is
Fernon 28:b7d01a55530f 229 OutRange2 = false;
Fernon 28:b7d01a55530f 230 }
Fernon 28:b7d01a55530f 231 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing
Fernon 28:b7d01a55530f 232 if (emg_value2 >= T2 ) {
Fernon 28:b7d01a55530f 233 Direction2 = 1;
Fernon 29:7653adbbb101 234 v2 = 0.05;
Fernon 28:b7d01a55530f 235 }
Fernon 28:b7d01a55530f 236 if (emg_value2 > T1 && emg_value2 < T2) {
Fernon 28:b7d01a55530f 237 Direction2 = 0;
Fernon 29:7653adbbb101 238 v2 = 0.05;
Fernon 28:b7d01a55530f 239 }
Fernon 28:b7d01a55530f 240 if (emg_value2 <= T1) {
Fernon 28:b7d01a55530f 241 Direction2 = 0;
Fernon 28:b7d01a55530f 242 v2 = 0;
Fernon 28:b7d01a55530f 243 }
Fernon 28:b7d01a55530f 244 }
Fernon 27:9cca2ad74ec0 245 if (Button2 == 0) { //Afvuren van de RBG
Fernon 27:9cca2ad74ec0 246 Fire();
Fernon 27:9cca2ad74ec0 247 }
Fernon 22:2e1713475f5f 248 }
Fernon 22:2e1713475f5f 249
Fernon 25:230bd4e1f3ef 250 while (Home) { //Terugkeren naar vaste positie
Fernon 25:230bd4e1f3ef 251 OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd.
Fernon 25:230bd4e1f3ef 252 Goal = 0;
Fernon 25:230bd4e1f3ef 253 Goal2 = 0;
Fernon 25:230bd4e1f3ef 254 if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) {
Fernon 21:d0156eadcbfe 255 timer.start();
Fernon 21:d0156eadcbfe 256 } else {
Fernon 21:d0156eadcbfe 257 timer.stop();
Fernon 21:d0156eadcbfe 258 timer.reset();
Fernon 16:b0ec8e6a8ad4 259 }
Fernon 22:2e1713475f5f 260 if (timer.read() >= 3) {
Fernon 22:2e1713475f5f 261 Home = false;
Fernon 18:0f7f57228901 262 Errori = 0;
Fernon 18:0f7f57228901 263 Errord = 0;
Fernon 25:230bd4e1f3ef 264 Errorp = 0;
Fernon 25:230bd4e1f3ef 265 Errori2 = 0;
Fernon 25:230bd4e1f3ef 266 Errord2 = 0;
Fernon 25:230bd4e1f3ef 267 Errorp2 = 0;
Fernon 11:a9a23042fc9e 268 }
Fernon 8:a2b725b502d8 269 }
Fernon 0:5a5f417fa1b2 270 }
Fernon 22:2e1713475f5f 271 }