Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
main.cpp@11:a9a23042fc9e, 2015-09-28 (annotated)
- Committer:
- Fernon
- Date:
- Mon Sep 28 21:16:40 2015 +0000
- Revision:
- 11:a9a23042fc9e
- Parent:
- 10:e210675cbe71
- Child:
- 12:dcf90cafb2a5
PD controller met button, V werkt nog niet perfect en button zorgt voor activatie.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Fernon | 0:5a5f417fa1b2 | 1 | #include "mbed.h" |
Fernon | 0:5a5f417fa1b2 | 2 | #include "QEI.h" |
Fernon | 8:a2b725b502d8 | 3 | #include "math.h" |
Fernon | 0:5a5f417fa1b2 | 4 | |
Fernon | 1:bb11e38dda43 | 5 | DigitalOut Direction(D4); //1 = CCW - 0 = CW |
Fernon | 1:bb11e38dda43 | 6 | PwmOut PowerMotor(D5); //van 0 tot 1 |
Fernon | 11:a9a23042fc9e | 7 | DigitalIn Button(D1); |
Fernon | 1:bb11e38dda43 | 8 | AnalogIn PotMeter(A1); |
Fernon | 1:bb11e38dda43 | 9 | QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder |
Fernon | 1:bb11e38dda43 | 10 | Serial pc(USBTX, USBRX); |
Fernon | 2:f0e9ffc5df09 | 11 | Ticker Pot; |
Fernon | 0:5a5f417fa1b2 | 12 | |
Fernon | 10:e210675cbe71 | 13 | double z=0; //initiele waarde potmeter |
Fernon | 8:a2b725b502d8 | 14 | const double twopi = 6.2831853071795; |
Fernon | 8:a2b725b502d8 | 15 | const double pi = twopi/2; |
Fernon | 8:a2b725b502d8 | 16 | int Pulses; |
Fernon | 10:e210675cbe71 | 17 | double Rotatie; //aantal graden dat de motor is gedraaid |
Fernon | 10:e210675cbe71 | 18 | double Rotatieup; //aantal graden dat de motor is gedraaid in een bereik van n*pi |
Fernon | 11:a9a23042fc9e | 19 | double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan |
Fernon | 8:a2b725b502d8 | 20 | double Error = 0; |
Fernon | 10:e210675cbe71 | 21 | double Errorv = 0; |
Fernon | 10:e210675cbe71 | 22 | double Kp = 2; //Moet berekend worden aan de hand van Control concept slides |
Fernon | 11:a9a23042fc9e | 23 | double Kd = 0.1; |
Fernon | 10:e210675cbe71 | 24 | double v = 0; //snelheid van de motor (0-0.1 |
Fernon | 10:e210675cbe71 | 25 | double upperlimit; //max aantal rotaties |
Fernon | 11:a9a23042fc9e | 26 | bool Excecute = false; |
Fernon | 10:e210675cbe71 | 27 | |
Fernon | 10:e210675cbe71 | 28 | double n = 2; |
Fernon | 2:f0e9ffc5df09 | 29 | |
Fernon | 5:d47e6a96256b | 30 | void readpot() |
Fernon | 2:f0e9ffc5df09 | 31 | { |
Fernon | 10:e210675cbe71 | 32 | z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld |
Fernon | 2:f0e9ffc5df09 | 33 | } |
Fernon | 0:5a5f417fa1b2 | 34 | |
Fernon | 8:a2b725b502d8 | 35 | |
Fernon | 0:5a5f417fa1b2 | 36 | int main() |
Fernon | 0:5a5f417fa1b2 | 37 | { |
Fernon | 11:a9a23042fc9e | 38 | upperlimit = n*twopi; |
Fernon | 2:f0e9ffc5df09 | 39 | pc.baud(115200); |
Fernon | 2:f0e9ffc5df09 | 40 | PowerMotor.write(0); |
Fernon | 6:cf20f04dbab4 | 41 | Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde |
Fernon | 0:5a5f417fa1b2 | 42 | while (true) { |
Fernon | 11:a9a23042fc9e | 43 | if (Button == 0) { |
Fernon | 11:a9a23042fc9e | 44 | Excecute =! Excecute; |
Fernon | 11:a9a23042fc9e | 45 | } |
Fernon | 11:a9a23042fc9e | 46 | |
Fernon | 11:a9a23042fc9e | 47 | while (Excecute ) { |
Fernon | 11:a9a23042fc9e | 48 | Pulses = Encoder.getPulses(); |
Fernon | 11:a9a23042fc9e | 49 | Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen |
Fernon | 11:a9a23042fc9e | 50 | Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit |
Fernon | 11:a9a23042fc9e | 51 | pc.printf ("Potmeter = %f\n", z); |
Fernon | 11:a9a23042fc9e | 52 | pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); |
Fernon | 11:a9a23042fc9e | 53 | Goal = z*upperlimit; // Het doel waar hij naar toe moet |
Fernon | 11:a9a23042fc9e | 54 | Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 11:a9a23042fc9e | 55 | Errorv = 0-v; |
Fernon | 11:a9a23042fc9e | 56 | if (Error >= 0) { //Bepalen van de rotatie richting |
Fernon | 11:a9a23042fc9e | 57 | Direction =0; |
Fernon | 11:a9a23042fc9e | 58 | } else { |
Fernon | 11:a9a23042fc9e | 59 | Direction =1; |
Fernon | 11:a9a23042fc9e | 60 | } |
Fernon | 11:a9a23042fc9e | 61 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 11:a9a23042fc9e | 62 | v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor |
Fernon | 11:a9a23042fc9e | 63 | PowerMotor.write(v); // Snelheid die naar de motor geschreven wordt (wordt alleen in een schaal van 0-1 weergegeven). (uit ervaring van Motor4_Setpoint loopt dit maar tussen 0-0.1). |
Fernon | 11:a9a23042fc9e | 64 | } |
Fernon | 8:a2b725b502d8 | 65 | } |
Fernon | 0:5a5f417fa1b2 | 66 | } |
Fernon | 3:af3f0ed8c99e | 67 | |
Fernon | 8:a2b725b502d8 | 68 | |
Fernon | 8:a2b725b502d8 | 69 |