EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
Diff: main.cpp
- Revision:
- 10:e210675cbe71
- Parent:
- 9:4743f3bb39b2
- Child:
- 11:a9a23042fc9e
diff -r 4743f3bb39b2 -r e210675cbe71 main.cpp --- a/main.cpp Mon Sep 28 20:13:03 2015 +0000 +++ b/main.cpp Mon Sep 28 21:01:31 2015 +0000 @@ -9,92 +9,54 @@ Serial pc(USBTX, USBRX); Ticker Pot; -double z=0; +double z=0; //initiele waarde potmeter const double twopi = 6.2831853071795; const double pi = twopi/2; int Pulses; -double Rotatie; -double Rotatietwopi; -double Goal = 0; +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 Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan double Error = 0; -double K = 2; -double v; +double Errorv = 0; +double Kp = 2; //Moet berekend worden aan de hand van Control concept slides +double Kd = 0.1; +double v = 0; //snelheid van de motor (0-0.1 +double upperlimit; //max aantal rotaties + + +double n = 2; void readpot() { - z = PotMeter.read()/10; + z = PotMeter.read(); //uitlezen Potmeter of voor EMG bijvoorbeeld } int main() { + 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 while (true) { Pulses = Encoder.getPulses(); - Rotatie = (Pulses*twopi)/4192; - Rotatietwopi = fmod(Rotatie,twopi); - - pc.printf ("Potmeter = %f\n", z);return 0; pc.printf ("Rotatie = %f [radialen] \n", Rotatietwopi); -// if (z > 0.05) { -// Goal = pi; -// } - Error = Goal-Rotatietwopi; - if (Error >= 0) { + 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 =0; } else { - Direction =1; + Direction =1; } pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - v = K*fabs(Error); - PowerMotor.write(v); + v = Kp*fabs(Error)+Kd*fabs(Errorv); // 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). } } - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -