Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 14:b9c925a8176a
- Parent:
- 13:c60942a1da8e
- Child:
- 15:f7e2b20f4dca
diff -r c60942a1da8e -r b9c925a8176a main.cpp --- a/main.cpp Wed Oct 07 13:30:46 2015 +0000 +++ b/main.cpp Wed Oct 07 14:48:06 2015 +0000 @@ -9,7 +9,7 @@ AnalogIn PotMeter(A1); Encoder MotorEncoder(D12,D13); //Encoder Serial pc(USBTX, USBRX); -Ticker Pot; +Ticker MotorWrite; HIDScope scope(3); double z=0; //initiele waarde potmeter @@ -17,19 +17,26 @@ const double pi = twopi/2; int Pulses; double Rotatie; //aantal graden dat de motor is gedraaid -double Rotatieup; //aantal graden dat de motor is gedraaid in een bereik van n*pi +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 readpot() +void MotorSet() { - z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld + v=Kp*Error; + if (v>=0) { + Direction=1; + } else { + Direction=0; + } + PowerMotor.write(fabs(v)); } int main() @@ -37,7 +44,7 @@ upperlimit = n*twopi; pc.baud(115200); PowerMotor.write(0); - Pot.attach(readpot,0.1); // Deze ticker moet de waarde uitlezen van de PotMeter 10 keer per seconde + 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; @@ -48,20 +55,13 @@ 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 + 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 - if (Error >= 0) { //Bepalen van de rotatie richting - Direction =1; - } else { - Direction =0; + pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + if (Error>=-0.1 && Error<=0.1) { + Excecute=false; + PowerMotor.write(0); } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - 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; - //} scope.set(0,Goal); scope.set(1,Rotatieup); scope.set(2,Error);