blaa
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of week6ordenenscript by
Diff: main.cpp
- Revision:
- 8:e125b9fa77b9
- Parent:
- 7:05495acc08b0
- Child:
- 13:290a1c5d4230
diff -r 05495acc08b0 -r e125b9fa77b9 main.cpp --- a/main.cpp Wed Nov 01 14:59:42 2017 +0000 +++ b/main.cpp Thu Nov 02 18:18:44 2017 +0000 @@ -28,6 +28,10 @@ Encoder motor2(D9,D8,true); Ticker DubbelTreecko; +//motors +//float Huidigepositie2; +//float Huidigepositie; + float PwmPeriod2 = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) float e_prev2 = 0; float e_int2 = 0; @@ -46,13 +50,13 @@ { float potmeterwaarde2 = potmeter1.read(); int maxwaarde2 = 4096; // = 64x64 - float refP2 = potmeterwaarde2*maxwaarde2; + float refP2 = -potmeterwaarde2*maxwaarde2; return refP2; // value between 0 and 4096 } float FeedBackControl(float error, float &e_prev, float &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { - float kp = 0.001; // kind of scaled. + float kp = 0.001; // kind of scaled. float Proportional= kp*error; float kd = 0.0004; // kind of scaled. @@ -71,15 +75,15 @@ float FeedBackControl2(float error2, float &e_prev2, float &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) { - float kp2 = 0.001; // kind of scaled. + float kp2 = 0.1; // kind of scaled. float Proportional2= kp2*error2; - float kd2 = 0.0004; // kind of scaled. + float kd2 = 0.04; // kind of scaled. float VelocityError2 = (error2 - e_prev2)/Ts; float Derivative2 = kd2*VelocityError2; e_prev2 = error2; - float ki2 = 0.00005; // kind of scaled. + float ki2 = 0.05; // kind of scaled. e_int2 = e_int2+Ts*error2; float Integrator2 = ki2*e_int2; @@ -114,11 +118,11 @@ { if (motorValue2 >= 0) { - M2D = 0; + M2D = 1; } else { - M2D = 1; + M2D =0; } if (fabs(motorValue2) > 1) @@ -161,6 +165,7 @@ float error2 = (refP2 - Huidigepositie2);// make an error float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2); SetMotor2(motorValue2); + //pc.printf("encoder 2 = %f\r\n",Huidigepositie2); } @@ -170,15 +175,16 @@ Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. DubbelTreecko.attach(MeasureAndControl2, Ts); - + pc.baud(115200); while(1) { wait(0.2); - pc.baud(115200); - float B = motor1.getPosition(); - float Potmeterwaarde = potMeter2.read(); + // pc.printf(" encoder 1 %f, encoder 2 %f\r\n",Huidigepositie,Huidigepositie2); + + //float B = motor1.getPosition(); + //float Potmeterwaarde = potMeter2.read(); //float positie = B%4096; - //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V + // pc.printf("pos: %f, \r\n pos2 = %f",motor1.getPosition(),motor2.getPosition); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V } }