Fernon Eijkhoudt / Mbed 2 deprecated ROBOBIRDS_FINAL

EMG driven robot which shoots elastic bands

Dependencies:   QEI mbed

Fork of RoboBirdV1 by Fernon Eijkhoudt

main.cpp@15:f7e2b20f4dca, 2015-10-07 (annotated)

Committer:
yc238
Date:
Wed Oct 07 17:24:39 2015 +0000
Revision:
15:f7e2b20f4dca
Parent:
14:b9c925a8176a
Child:
16:b0ec8e6a8ad4
Zou moeten werken

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 15:f7e2b20f4dca 8 PwmOut PowerServo(D7);
yc238 13:c60942a1da8e 9 DigitalIn Button(D3);
yc238 15:f7e2b20f4dca 10 DigitalIn Button2(D2);
Fernon 1:bb11e38dda43 11 AnalogIn PotMeter(A1);
yc238 13:c60942a1da8e 12 Encoder MotorEncoder(D12,D13); //Encoder
Fernon 1:bb11e38dda43 13 Serial pc(USBTX, USBRX);
yc238 14:b9c925a8176a 14 Ticker MotorWrite;
yc238 15:f7e2b20f4dca 15 Ticker Sender;
yc238 13:c60942a1da8e 16 HIDScope scope(3);
Fernon 0:5a5f417fa1b2 17
yc238 15:f7e2b20f4dca 18
Fernon 10:e210675cbe71 19 double z=0; //initiele waarde potmeter
Fernon 8:a2b725b502d8 20 const double twopi = 6.2831853071795;
Fernon 8:a2b725b502d8 21 const double pi = twopi/2;
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;
yc238 14:b9c925a8176a 31 double Fs=100;
Fernon 10:e210675cbe71 32
yc238 15:f7e2b20f4dca 33 double n = 3;
Fernon 2:f0e9ffc5df09 34
yc238 14:b9c925a8176a 35 void MotorSet()
Fernon 2:f0e9ffc5df09 36 {
yc238 14:b9c925a8176a 37 v=Kp*Error;
yc238 14:b9c925a8176a 38 if (v>=0) {
yc238 14:b9c925a8176a 39 Direction=1;
yc238 14:b9c925a8176a 40 } else {
yc238 14:b9c925a8176a 41 Direction=0;
yc238 14:b9c925a8176a 42 }
yc238 14:b9c925a8176a 43 PowerMotor.write(fabs(v));
Fernon 2:f0e9ffc5df09 44 }
yc238 15:f7e2b20f4dca 45 void Send()
yc238 15:f7e2b20f4dca 46 {
yc238 15:f7e2b20f4dca 47 scope.set(0,Goal);
yc238 15:f7e2b20f4dca 48 scope.set(1,Rotatieup);
yc238 15:f7e2b20f4dca 49 scope.set(2,Error);
yc238 15:f7e2b20f4dca 50 // scope.set(2,v);
yc238 15:f7e2b20f4dca 51 scope.send();
yc238 15:f7e2b20f4dca 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);
yc238 15:f7e2b20f4dca 58 Sender.attach(Send,1/Fs/2);
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 11:a9a23042fc9e 61 if (Button == 0) {
Fernon 11:a9a23042fc9e 62 Excecute =! Excecute;
yc238 12:dcf90cafb2a5 63 }
yc238 12:dcf90cafb2a5 64 while (Excecute ) {
yc238 13:c60942a1da8e 65 Pulses = MotorEncoder.getPosition();
yc238 12:dcf90cafb2a5 66 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
yc238 12:dcf90cafb2a5 67 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
yc238 12:dcf90cafb2a5 68 pc.printf ("Potmeter = %f\n", z);
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);
yc238 15:f7e2b20f4dca 73 if (Error>=-0.001 && Error<=0.001) {
yc238 14:b9c925a8176a 74 Excecute=false;
yc238 15:f7e2b20f4dca 75 v=0;
Fernon 11:a9a23042fc9e 76 }
Fernon 8:a2b725b502d8 77 }
Fernon 0:5a5f417fa1b2 78 }
yc238 12:dcf90cafb2a5 79 }