Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 2:7ea5ae2287a7
- Parent:
- 1:a76fd17e18b3
- Child:
- 3:6e28b992b99e
diff -r a76fd17e18b3 -r 7ea5ae2287a7 main.cpp --- a/main.cpp Mon Oct 28 16:11:15 2019 +0000 +++ b/main.cpp Mon Oct 28 16:38:46 2019 +0000 @@ -14,6 +14,7 @@ AnalogIn potmeter2(A1); AnalogIn potmeter3(A2); AnalogIn potmeter4(A3); + // Encoder DigitalIn encA1(D9); DigitalIn encB1(D8); @@ -24,7 +25,8 @@ float Ts = 0.01; //Sample time float motor1angle; //Measured angle motor 1 float motor2angle; //Measured angle motor 2 -float omega1; //velocity rad/s motor 1 +float potmeter; +float omega1=1; //velocity rad/s motor 1 float omega2; //Velocity rad/s motor2 float deg2rad=0.0174532; //Conversion factor degree to rad float rad2deg=57.29578; //Conversion factor rad to degree @@ -120,13 +122,12 @@ void motorCalibration1() { - button1.fall(&togglehoek); motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s float potmeter=potmeter1.read(); - controlsignal1=potmeter; + controlsignal1=0.0980f; motor1Power.write(abs(controlsignal1*motortoggle)); - motor1Direction=1; + motor1Direction=0; //Dit moet je doen zolang omega motor 1 > 0.iets //State switch //Dan motor 2 tot omega < 0.iets @@ -134,13 +135,12 @@ void motorCalibration2(){ - float potmeter=potmeter1.read(); - button1.fall(&togglehoek); + potmeter=potmeter1.read(); motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s controlsignal1=potmeter; motor1Power.write(abs(controlsignal1*motortoggle)); - motor1Direction=1; + motor1Direction=0; motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s @@ -150,17 +150,6 @@ //Dit moet je doen zolang omega van motor 2 > 0.iets } -void Plotje(){ - scope.set(0,q1*rad2deg); //gewenste hoek - scope.set(1,motor1angle/5.5f*rad2deg); //Gemeten hoek - scope.set(2,motor1error/5.5f*rad2deg); //verschil in gewenste en gemeten hoek - scope.set(3,q2*rad2deg); //gewenste hoek - scope.set(4,motor2angle/5.5f*rad2deg); //Gemeten hoek - scope.set(5,motor2error/5.5f*rad2deg); //verschil in gewenste en gemeten hoek - - scope.send(); //send what's in scope memory to PC -} - void toggleMotor() { motortoggle = !motortoggle; @@ -172,15 +161,16 @@ pc.printf("\r\nStarting...\r\n\r\n"); motor1Power.period(PWM_period); motor2Power.period(PWM_period); + motorTicker.attach(motorCalibration1, 0.01); //Dit moet je doen zolang omega < 0.iets - motorTicker.attack(motorCalibration2, 0.01); - scopeTicker.attach(Plotje, 0.01); + //motorTicker.attack(motorCalibration2, 0.01); encoderTicker.attach(readEncoder, Ts); button2.fall(&toggleMotor); while (true) { - pc.printf("Omega1: %f Omega 2: %f Potmeter: %f \r\n", omega1, omega2, potmeter); + potmeter=potmeter1.read(); + pc.printf("Omega1: %f Omega 2: %f controlsignal1: %f \r\n", omega1, omega2, controlsignal1); wait(0.5); } }