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@14:b9c925a8176a, 2015-10-07 (annotated)

Committer:
yc238
Date:
Wed Oct 07 14:48:06 2015 +0000
Revision:
14:b9c925a8176a
Parent:
13:c60942a1da8e
Child:
15:f7e2b20f4dca
Werkt, met het updaten van de motor op een vaste interval

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Fernon 0:5a5f417fa1b2 1 #include "mbed.h"
yc238 13:c60942a1da8e 2 #include "encoder.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 13:c60942a1da8e 8 DigitalIn Button(D3);
Fernon 1:bb11e38dda43 9 AnalogIn PotMeter(A1);
yc238 13:c60942a1da8e 10 Encoder MotorEncoder(D12,D13); //Encoder
Fernon 1:bb11e38dda43 11 Serial pc(USBTX, USBRX);
yc238 14:b9c925a8176a 12 Ticker MotorWrite;
yc238 13:c60942a1da8e 13 HIDScope scope(3);
Fernon 0:5a5f417fa1b2 14
Fernon 10:e210675cbe71 15 double z=0; //initiele waarde potmeter
Fernon 8:a2b725b502d8 16 const double twopi = 6.2831853071795;
Fernon 8:a2b725b502d8 17 const double pi = twopi/2;
Fernon 8:a2b725b502d8 18 int Pulses;
Fernon 10:e210675cbe71 19 double Rotatie; //aantal graden dat de motor is gedraaid
yc238 14:b9c925a8176a 20 double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi
Fernon 11:a9a23042fc9e 21 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 8:a2b725b502d8 22 double Error = 0;
yc238 13:c60942a1da8e 23 double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides
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;
yc238 14:b9c925a8176a 27 double Fs=100;
Fernon 10:e210675cbe71 28
yc238 13:c60942a1da8e 29 double n = 8;
Fernon 2:f0e9ffc5df09 30
yc238 14:b9c925a8176a 31 void MotorSet()
Fernon 2:f0e9ffc5df09 32 {
yc238 14:b9c925a8176a 33 v=Kp*Error;
yc238 14:b9c925a8176a 34 if (v>=0) {
yc238 14:b9c925a8176a 35 Direction=1;
yc238 14:b9c925a8176a 36 } else {
yc238 14:b9c925a8176a 37 Direction=0;
yc238 14:b9c925a8176a 38 }
yc238 14:b9c925a8176a 39 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 40 }
Fernon 0:5a5f417fa1b2 41
Fernon 0:5a5f417fa1b2 42 int main()
Fernon 0:5a5f417fa1b2 43 {
Fernon 11:a9a23042fc9e 44 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 45 pc.baud(115200);
Fernon 2:f0e9ffc5df09 46 PowerMotor.write(0);
yc238 14:b9c925a8176a 47 MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
Fernon 0:5a5f417fa1b2 48 while (true) {
Fernon 11:a9a23042fc9e 49 if (Button == 0) {
Fernon 11:a9a23042fc9e 50 Excecute =! Excecute;
yc238 12:dcf90cafb2a5 51 }
yc238 12:dcf90cafb2a5 52 while (Excecute ) {
yc238 13:c60942a1da8e 53 Pulses = MotorEncoder.getPosition();
yc238 12:dcf90cafb2a5 54 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
yc238 12:dcf90cafb2a5 55 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
yc238 12:dcf90cafb2a5 56 pc.printf ("Potmeter = %f\n", z);
yc238 12:dcf90cafb2a5 57 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
yc238 14:b9c925a8176a 58 Goal = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet
yc238 12:dcf90cafb2a5 59 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
yc238 14:b9c925a8176a 60 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
yc238 14:b9c925a8176a 61 if (Error>=-0.1 && Error<=0.1) {
yc238 14:b9c925a8176a 62 Excecute=false;
yc238 14:b9c925a8176a 63 PowerMotor.write(0);
Fernon 11:a9a23042fc9e 64 }
yc238 12:dcf90cafb2a5 65 scope.set(0,Goal);
yc238 12:dcf90cafb2a5 66 scope.set(1,Rotatieup);
yc238 13:c60942a1da8e 67 scope.set(2,Error);
yc238 12:dcf90cafb2a5 68 scope.send();
Fernon 8:a2b725b502d8 69 }
Fernon 0:5a5f417fa1b2 70 }
yc238 12:dcf90cafb2a5 71 }