Alles in 1
Dependencies: Encoder HIDScope MODSERIAL QEI mbed
Fork of RoboBird3 by
main.cpp@28:b7d01a55530f, 2015-10-16 (annotated)
- Committer:
- Fernon
- Date:
- Fri Oct 16 09:27:42 2015 +0000
- Revision:
- 28:b7d01a55530f
- Parent:
- 27:9cca2ad74ec0
- Child:
- 29:7653adbbb101
Motor 1 en 2 werken beide, alleen de aansturing loopt nog niet lekker. Zo gaat de motor 2 zonder limit door. en heeft potmeter 1 rare artefacten.
Who changed what in which revision?
User | Revision | Line number | New 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 | 28:b7d01a55530f | 14 | QEI BIER(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 | 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 | 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 | 28:b7d01a55530f | 123 | Direction2 = 1; |
Fernon | 27:9cca2ad74ec0 | 124 | } else { |
Fernon | 28:b7d01a55530f | 125 | Direction2 = 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 | 28:b7d01a55530f | 135 | Pulses2 = BIER.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 | 28:b7d01a55530f | 175 | PowerMotor2.write(0); |
Fernon | 16:b0ec8e6a8ad4 | 176 | Sender.attach(Send,0.5/Fs); |
yc238 | 15:f7e2b20f4dca | 177 | MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde |
Fernon | 23:c97e206cf2a7 | 178 | sampleEMG.attach(EMGsample,0.5/Fs); |
Fernon | 23:c97e206cf2a7 | 179 | PowerServo.period_ms(20); |
Fernon | 0:5a5f417fa1b2 | 180 | while (true) { |
Fernon | 17:c5eea26e171d | 181 | Encoder.reset(); |
Fernon | 28:b7d01a55530f | 182 | BIER.reset(); |
Fernon | 11:a9a23042fc9e | 183 | if (Button == 0) { |
Fernon | 11:a9a23042fc9e | 184 | Excecute =! Excecute; |
Fernon | 17:c5eea26e171d | 185 | } |
Fernon | 25:230bd4e1f3ef | 186 | while (Excecute ) { |
Fernon | 25:230bd4e1f3ef | 187 | // Eerst wordt motor 1 aangestuurd |
Fernon | 26:5b6c577fb3c1 | 188 | pc.printf("Rotatie = %f \n", Rotatie); |
Fernon | 27:9cca2ad74ec0 | 189 | pc.printf("Rotatie2 = %f \n", Rotatie2); |
Fernon | 25:230bd4e1f3ef | 190 | if (Rotatie >= upperlimit) { //Als hij buiten bereik is |
Fernon | 27:9cca2ad74ec0 | 191 | Goal = upperlimit - margin; |
Fernon | 23:c97e206cf2a7 | 192 | OutRange = true; |
Fernon | 22:2e1713475f5f | 193 | } |
Fernon | 25:230bd4e1f3ef | 194 | if (Rotatie <= downlimit) { //Als hij buiten bereik is |
Fernon | 27:9cca2ad74ec0 | 195 | Goal = 0 + margin; |
Fernon | 23:c97e206cf2a7 | 196 | OutRange = true; |
Fernon | 22:2e1713475f5f | 197 | } |
Fernon | 25:230bd4e1f3ef | 198 | if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is |
Fernon | 23:c97e206cf2a7 | 199 | OutRange = false; |
Fernon | 24:711c7c388e57 | 200 | } |
Fernon | 25:230bd4e1f3ef | 201 | if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing |
Fernon | 23:c97e206cf2a7 | 202 | if (emg_value >= T2 ) { |
Fernon | 22:2e1713475f5f | 203 | Direction = 1; |
Fernon | 28:b7d01a55530f | 204 | v = 0.1; |
Fernon | 22:2e1713475f5f | 205 | } |
Fernon | 28:b7d01a55530f | 206 | if (emg_value > T1 && emg_value < T2) { |
Fernon | 22:2e1713475f5f | 207 | Direction = 0; |
Fernon | 28:b7d01a55530f | 208 | v = 0.1; |
Fernon | 24:711c7c388e57 | 209 | } |
Fernon | 24:711c7c388e57 | 210 | if (emg_value <= T1) { |
Fernon | 23:c97e206cf2a7 | 211 | Direction = 0; |
Fernon | 23:c97e206cf2a7 | 212 | v = 0; |
Fernon | 23:c97e206cf2a7 | 213 | } |
Fernon | 23:c97e206cf2a7 | 214 | } |
Fernon | 24:711c7c388e57 | 215 | |
Fernon | 28:b7d01a55530f | 216 | // Vanaf hier wordt motor 2 aangestuurd |
Fernon | 28:b7d01a55530f | 217 | if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is |
Fernon | 28:b7d01a55530f | 218 | Goal2 = turnlimit; |
Fernon | 28:b7d01a55530f | 219 | OutRange2 = true; |
Fernon | 28:b7d01a55530f | 220 | } |
Fernon | 28:b7d01a55530f | 221 | if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is |
Fernon | 28:b7d01a55530f | 222 | Goal2 = -turnlimit; |
Fernon | 28:b7d01a55530f | 223 | OutRange2 = true; |
Fernon | 28:b7d01a55530f | 224 | } |
Fernon | 28:b7d01a55530f | 225 | if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is |
Fernon | 28:b7d01a55530f | 226 | OutRange2 = false; |
Fernon | 28:b7d01a55530f | 227 | } |
Fernon | 28:b7d01a55530f | 228 | if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing |
Fernon | 28:b7d01a55530f | 229 | if (emg_value2 >= T2 ) { |
Fernon | 28:b7d01a55530f | 230 | Direction2 = 1; |
Fernon | 28:b7d01a55530f | 231 | v2 = 0.1; |
Fernon | 28:b7d01a55530f | 232 | } |
Fernon | 28:b7d01a55530f | 233 | if (emg_value2 > T1 && emg_value2 < T2) { |
Fernon | 28:b7d01a55530f | 234 | Direction2 = 0; |
Fernon | 28:b7d01a55530f | 235 | v2 = 0.1; |
Fernon | 28:b7d01a55530f | 236 | } |
Fernon | 28:b7d01a55530f | 237 | if (emg_value2 <= T1) { |
Fernon | 28:b7d01a55530f | 238 | Direction2 = 0; |
Fernon | 28:b7d01a55530f | 239 | v2 = 0; |
Fernon | 28:b7d01a55530f | 240 | } |
Fernon | 28:b7d01a55530f | 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 | } |