EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
main.cpp@17:c5eea26e171d, 2015-10-07 (annotated)
- Committer:
- Fernon
- Date:
- Wed Oct 07 18:25:02 2015 +0000
- Revision:
- 17:c5eea26e171d
- Parent:
- 16:b0ec8e6a8ad4
- Child:
- 18:0f7f57228901
Working P controller with return after 5 seconds to home.
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 | 1:bb11e38dda43 | 6 | DigitalOut Direction(D4); //1 = CCW - 0 = CW |
Fernon | 1:bb11e38dda43 | 7 | PwmOut PowerMotor(D5); //van 0 tot 1 |
yc238 | 15:f7e2b20f4dca | 8 | PwmOut PowerServo(D7); |
Fernon | 16:b0ec8e6a8ad4 | 9 | DigitalIn Button(PTC6); |
Fernon | 16:b0ec8e6a8ad4 | 10 | //DigitalIn Button2(D2); |
Fernon | 1:bb11e38dda43 | 11 | AnalogIn PotMeter(A1); |
Fernon | 16:b0ec8e6a8ad4 | 12 | QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder |
Fernon | 1:bb11e38dda43 | 13 | Serial pc(USBTX, USBRX); |
yc238 | 14:b9c925a8176a | 14 | Ticker MotorWrite; |
yc238 | 15:f7e2b20f4dca | 15 | Ticker Sender; |
Fernon | 16:b0ec8e6a8ad4 | 16 | Timer timer; |
yc238 | 13:c60942a1da8e | 17 | HIDScope scope(3); |
Fernon | 0:5a5f417fa1b2 | 18 | |
yc238 | 15:f7e2b20f4dca | 19 | |
Fernon | 10:e210675cbe71 | 20 | double z=0; //initiele waarde potmeter |
Fernon | 8:a2b725b502d8 | 21 | const double twopi = 6.2831853071795; |
Fernon | 8:a2b725b502d8 | 22 | int Pulses; |
Fernon | 10:e210675cbe71 | 23 | double Rotatie; //aantal graden dat de motor is gedraaid |
yc238 | 14:b9c925a8176a | 24 | double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi |
Fernon | 11:a9a23042fc9e | 25 | double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan |
Fernon | 8:a2b725b502d8 | 26 | double Error = 0; |
yc238 | 13:c60942a1da8e | 27 | double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides |
Fernon | 10:e210675cbe71 | 28 | double v = 0; //snelheid van de motor (0-0.1 |
Fernon | 10:e210675cbe71 | 29 | double upperlimit; //max aantal rotaties |
Fernon | 11:a9a23042fc9e | 30 | bool Excecute = false; |
Fernon | 17:c5eea26e171d | 31 | bool Excecute2 = false; |
yc238 | 14:b9c925a8176a | 32 | double Fs=100; |
Fernon | 10:e210675cbe71 | 33 | |
yc238 | 15:f7e2b20f4dca | 34 | double n = 3; |
Fernon | 2:f0e9ffc5df09 | 35 | |
yc238 | 14:b9c925a8176a | 36 | void MotorSet() |
Fernon | 2:f0e9ffc5df09 | 37 | { |
yc238 | 14:b9c925a8176a | 38 | v=Kp*Error; |
yc238 | 14:b9c925a8176a | 39 | if (v>=0) { |
yc238 | 14:b9c925a8176a | 40 | Direction=1; |
yc238 | 14:b9c925a8176a | 41 | } else { |
yc238 | 14:b9c925a8176a | 42 | Direction=0; |
yc238 | 14:b9c925a8176a | 43 | } |
yc238 | 14:b9c925a8176a | 44 | PowerMotor.write(fabs(v)); |
Fernon | 2:f0e9ffc5df09 | 45 | } |
yc238 | 15:f7e2b20f4dca | 46 | void Send() |
yc238 | 15:f7e2b20f4dca | 47 | { |
yc238 | 15:f7e2b20f4dca | 48 | scope.set(0,Goal); |
yc238 | 15:f7e2b20f4dca | 49 | scope.set(1,Rotatieup); |
yc238 | 15:f7e2b20f4dca | 50 | scope.set(2,Error); |
Fernon | 16:b0ec8e6a8ad4 | 51 | scope.send(); |
Fernon | 16:b0ec8e6a8ad4 | 52 | } |
Fernon | 0:5a5f417fa1b2 | 53 | int main() |
Fernon | 0:5a5f417fa1b2 | 54 | { |
Fernon | 11:a9a23042fc9e | 55 | upperlimit = n*twopi; |
Fernon | 2:f0e9ffc5df09 | 56 | pc.baud(115200); |
Fernon | 2:f0e9ffc5df09 | 57 | PowerMotor.write(0); |
Fernon | 16:b0ec8e6a8ad4 | 58 | Sender.attach(Send,0.5/Fs); |
yc238 | 15:f7e2b20f4dca | 59 | MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde |
Fernon | 0:5a5f417fa1b2 | 60 | while (true) { |
Fernon | 17:c5eea26e171d | 61 | Encoder.reset(); |
Fernon | 11:a9a23042fc9e | 62 | if (Button == 0) { |
Fernon | 11:a9a23042fc9e | 63 | Excecute =! Excecute; |
Fernon | 17:c5eea26e171d | 64 | } |
yc238 | 12:dcf90cafb2a5 | 65 | while (Excecute ) { |
Fernon | 16:b0ec8e6a8ad4 | 66 | Pulses = Encoder.getPulses(); |
yc238 | 12:dcf90cafb2a5 | 67 | Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen |
yc238 | 12:dcf90cafb2a5 | 68 | Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit |
yc238 | 12:dcf90cafb2a5 | 69 | pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); |
yc238 | 14:b9c925a8176a | 70 | Goal = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet |
yc238 | 12:dcf90cafb2a5 | 71 | Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal |
yc238 | 14:b9c925a8176a | 72 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 16:b0ec8e6a8ad4 | 73 | if (fabs(Error)<=0.15) { |
Fernon | 16:b0ec8e6a8ad4 | 74 | timer.start(); |
Fernon | 16:b0ec8e6a8ad4 | 75 | } else { |
Fernon | 16:b0ec8e6a8ad4 | 76 | timer.stop(); |
Fernon | 16:b0ec8e6a8ad4 | 77 | timer.reset(); |
Fernon | 16:b0ec8e6a8ad4 | 78 | } |
Fernon | 16:b0ec8e6a8ad4 | 79 | if (timer.read() >= 5) { |
yc238 | 14:b9c925a8176a | 80 | Excecute=false; |
Fernon | 17:c5eea26e171d | 81 | Excecute2 = true; |
Fernon | 17:c5eea26e171d | 82 | Error = 0; |
Fernon | 17:c5eea26e171d | 83 | } |
Fernon | 17:c5eea26e171d | 84 | } |
Fernon | 17:c5eea26e171d | 85 | while (Excecute2) { |
Fernon | 17:c5eea26e171d | 86 | Pulses = Encoder.getPulses(); |
Fernon | 17:c5eea26e171d | 87 | Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen |
Fernon | 17:c5eea26e171d | 88 | Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit |
Fernon | 17:c5eea26e171d | 89 | pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); |
Fernon | 17:c5eea26e171d | 90 | Goal = 0; // Het doel waar hij naar toe moet |
Fernon | 17:c5eea26e171d | 91 | Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal |
Fernon | 17:c5eea26e171d | 92 | pc.printf("Error = %f\n Goal = %f\n", Error, Goal); |
Fernon | 17:c5eea26e171d | 93 | if (fabs(Error)<=0.15) { |
Fernon | 17:c5eea26e171d | 94 | timer.start(); |
Fernon | 17:c5eea26e171d | 95 | } else { |
Fernon | 17:c5eea26e171d | 96 | timer.stop(); |
Fernon | 17:c5eea26e171d | 97 | timer.reset(); |
Fernon | 17:c5eea26e171d | 98 | } |
Fernon | 17:c5eea26e171d | 99 | if (timer.read() >= 5) { |
Fernon | 17:c5eea26e171d | 100 | Excecute2=false; |
Fernon | 17:c5eea26e171d | 101 | Error = 0; |
Fernon | 11:a9a23042fc9e | 102 | } |
Fernon | 8:a2b725b502d8 | 103 | } |
Fernon | 0:5a5f417fa1b2 | 104 | } |
Fernon | 17:c5eea26e171d | 105 | } |