Alles in 1
Dependencies: Encoder HIDScope MODSERIAL QEI mbed
Fork of RoboBird3 by
main.cpp
- Committer:
- yc238
- Date:
- 2015-10-07
- Revision:
- 14:b9c925a8176a
- Parent:
- 13:c60942a1da8e
- Child:
- 15:f7e2b20f4dca
File content as of revision 14:b9c925a8176a:
#include "mbed.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(D3); AnalogIn PotMeter(A1); Encoder MotorEncoder(D12,D13); //Encoder Serial pc(USBTX, USBRX); Ticker MotorWrite; HIDScope scope(3); double z=0; //initiele waarde potmeter const double twopi = 6.2831853071795; const double pi = twopi/2; int Pulses; double Rotatie; //aantal graden dat de motor is gedraaid double Rotatieup=0; //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 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 Fs=100; double n = 8; void MotorSet() { v=Kp*Error; if (v>=0) { Direction=1; } else { Direction=0; } PowerMotor.write(fabs(v)); } int main() { upperlimit = n*twopi; pc.baud(115200); PowerMotor.write(0); MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde while (true) { if (Button == 0) { Excecute =! Excecute; } while (Excecute ) { 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 = PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal pc.printf("Error = %f\n Goal = %f\n", Error, Goal); if (Error>=-0.1 && Error<=0.1) { Excecute=false; PowerMotor.write(0); } scope.set(0,Goal); scope.set(1,Rotatieup); scope.set(2,Error); scope.send(); } } }