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@10:e210675cbe71, 2015-09-28 (annotated)

Committer:
Fernon
Date:
Mon Sep 28 21:01:31 2015 +0000
Revision:
10:e210675cbe71
Parent:
9:4743f3bb39b2
Child:
11:a9a23042fc9e
PD controller (works but misses a thing with the Kd and the Errorv)

Who changed what in which revision?

UserRevisionLine numberNew 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 1:bb11e38dda43 7 AnalogIn PotMeter(A1);
Fernon 1:bb11e38dda43 8 QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder
Fernon 1:bb11e38dda43 9 Serial pc(USBTX, USBRX);
Fernon 2:f0e9ffc5df09 10 Ticker Pot;
Fernon 0:5a5f417fa1b2 11
Fernon 10:e210675cbe71 12 double z=0; //initiele waarde potmeter
Fernon 8:a2b725b502d8 13 const double twopi = 6.2831853071795;
Fernon 8:a2b725b502d8 14 const double pi = twopi/2;
Fernon 8:a2b725b502d8 15 int Pulses;
Fernon 10:e210675cbe71 16 double Rotatie; //aantal graden dat de motor is gedraaid
Fernon 10:e210675cbe71 17 double Rotatieup; //aantal graden dat de motor is gedraaid in een bereik van n*pi
Fernon 10:e210675cbe71 18 double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan
Fernon 8:a2b725b502d8 19 double Error = 0;
Fernon 10:e210675cbe71 20 double Errorv = 0;
Fernon 10:e210675cbe71 21 double Kp = 2; //Moet berekend worden aan de hand van Control concept slides
Fernon 10:e210675cbe71 22 double Kd = 0.1;
Fernon 10:e210675cbe71 23 double v = 0; //snelheid van de motor (0-0.1
Fernon 10:e210675cbe71 24 double upperlimit; //max aantal rotaties
Fernon 10:e210675cbe71 25
Fernon 10:e210675cbe71 26
Fernon 10:e210675cbe71 27 double n = 2;
Fernon 2:f0e9ffc5df09 28
Fernon 5:d47e6a96256b 29 void readpot()
Fernon 2:f0e9ffc5df09 30 {
Fernon 10:e210675cbe71 31 z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld
Fernon 2:f0e9ffc5df09 32 }
Fernon 0:5a5f417fa1b2 33
Fernon 8:a2b725b502d8 34
Fernon 0:5a5f417fa1b2 35 int main()
Fernon 0:5a5f417fa1b2 36 {
Fernon 10:e210675cbe71 37 upperlimit = n*twopi;
Fernon 2:f0e9ffc5df09 38 pc.baud(115200);
Fernon 2:f0e9ffc5df09 39 PowerMotor.write(0);
Fernon 6:cf20f04dbab4 40 Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde
Fernon 0:5a5f417fa1b2 41 while (true) {
Fernon 8:a2b725b502d8 42 Pulses = Encoder.getPulses();
Fernon 10:e210675cbe71 43 Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen
Fernon 10:e210675cbe71 44 Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit
Fernon 10:e210675cbe71 45 pc.printf ("Potmeter = %f\n", z);
Fernon 10:e210675cbe71 46 pc.printf ("Rotatie = %f [radialen] \n", Rotatieup);
Fernon 10:e210675cbe71 47 Goal = z*upperlimit; // Het doel waar hij naar toe moet
Fernon 10:e210675cbe71 48 Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal
Fernon 10:e210675cbe71 49 Errorv = 0-v;
Fernon 10:e210675cbe71 50 if (Error >= 0) { //Bepalen van de rotatie richting
Fernon 8:a2b725b502d8 51 Direction =0;
Fernon 8:a2b725b502d8 52 } else {
Fernon 10:e210675cbe71 53 Direction =1;
Fernon 8:a2b725b502d8 54 }
Fernon 8:a2b725b502d8 55 pc.printf("Error = %f\n Goal = %f\n", Error, Goal);
Fernon 10:e210675cbe71 56 v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor
Fernon 10:e210675cbe71 57 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 0:5a5f417fa1b2 58 }
Fernon 2:f0e9ffc5df09 59 }
Fernon 3:af3f0ed8c99e 60
Fernon 8:a2b725b502d8 61
Fernon 8:a2b725b502d8 62