Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 13:c60942a1da8e
- Parent:
- 12:dcf90cafb2a5
- Child:
- 14:b9c925a8176a
--- a/main.cpp Wed Sep 30 19:25:46 2015 +0000 +++ b/main.cpp Wed Oct 07 13:30:46 2015 +0000 @@ -1,16 +1,16 @@ #include "mbed.h" -#include "QEI.h" +#include "encoder.h" #include "math.h" #include "HIDScope.h" DigitalOut Direction(D4); //1 = CCW - 0 = CW PwmOut PowerMotor(D5); //van 0 tot 1 -DigitalIn Button(D1); +DigitalIn Button(D3); AnalogIn PotMeter(A1); -QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder +Encoder MotorEncoder(D12,D13); //Encoder Serial pc(USBTX, USBRX); Ticker Pot; -HIDScope scope(2); +HIDScope scope(3); double z=0; //initiele waarde potmeter const double twopi = 6.2831853071795; @@ -20,14 +20,12 @@ double Rotatieup; //aantal graden dat de motor is gedraaid in een bereik van n*pi double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan double Error = 0; -double Errorv = 0; -double Kp = 2; //Moet berekend worden aan de hand van Control concept slides -double Kd = 0.1; +double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides double v = 0; //snelheid van de motor (0-0.1 double upperlimit; //max aantal rotaties bool Excecute = false; -double n = 2; +double n = 8; void readpot() { @@ -45,28 +43,28 @@ Excecute =! Excecute; } while (Excecute ) { - Pulses = Encoder.getPulses(); + Pulses = MotorEncoder.getPosition(); Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit pc.printf ("Potmeter = %f\n", z); pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); Goal = z*upperlimit; // Het doel waar hij naar toe moet Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal - Errorv = 0-v; if (Error >= 0) { //Bepalen van de rotatie richting + Direction =1; + } else { Direction =0; - } else { - Direction =1; } pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - v = Kp*fabs(Error)+Kd*fabs(Errorv); // Snelheid van motor + v = Kp*fabs(Error); // Snelheid van motor 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). - if (Error<=0.001 && Error>=-0.001) { - PowerMotor.write(0); - Excecute=false; - } + //if (Error<=0.001 && Error>=-0.001) { +// PowerMotor.write(0); +// Excecute=false; + //} scope.set(0,Goal); scope.set(1,Rotatieup); + scope.set(2,Error); scope.send(); } }