Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 10:990287a722d2
- Parent:
- 9:298469a70280
- Child:
- 11:cd7be08f46b0
diff -r 298469a70280 -r 990287a722d2 main.cpp --- a/main.cpp Tue Oct 29 18:55:53 2019 +0000 +++ b/main.cpp Tue Oct 29 20:05:39 2019 +0000 @@ -23,7 +23,7 @@ QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING); //Encoding motor 1 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING); //Encoding motor 2 float Ts = 0.01; //Sample time -float motor1angle; //Measured angle motor 1 +float motor1angle=-135.0*0.0174532*5.5; //Measured angle motor 1 float motor2angle; //Measured angle motor 2 float motor1offset; //Offset bij calibratie float motor2offset; @@ -43,8 +43,8 @@ //Motorcontrol bool motordir1; bool motordir2; -float motor1ref=0.1745; -float motor2ref=0.0873; +float motor1ref=-135.0*0.0174532*5.5; +float motor2ref; double controlsignal1; double controlsignal2; double pi2= 6.283185; @@ -108,6 +108,10 @@ volatile int counts1; // Encoder counts volatile int counts2; +volatile int counts1af; +volatile int counts2af; +int counts1offset; +int counts2offset; volatile int countsPrev1 = 0; volatile int countsPrev2 = 0; volatile int deltaCounts1; @@ -129,13 +133,13 @@ // Ticker Functions void readEncoder() { - counts1 = encoder1.getPulses(); - deltaCounts1 = counts1 - countsPrev1; - countsPrev1 = counts1; + counts1af = encoder1.getPulses(); + deltaCounts1 = counts1af - countsPrev1; + countsPrev1 = counts1af; - counts2 = encoder2.getPulses(); - deltaCounts2 = counts2 - countsPrev2; - countsPrev2 = counts2; + counts2af = encoder2.getPulses(); + deltaCounts2 = counts2af - countsPrev2; + countsPrev2 = counts2af; } void PID_controller(){ @@ -242,6 +246,7 @@ void motorControl(){ + counts1= counts1af-counts1offset; motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s motor1error=motor1ref-motor1angle; @@ -252,6 +257,7 @@ motor1Power.write(abs(controlsignal1)); motor1Direction= motordir1; + counts2= counts2af - counts2offset; motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s motor2error=motor2ref-motor2angle; @@ -290,8 +296,8 @@ } motor1Power.write(0.0f); motor2Power.write(0.0f); - motor1offset = (counts1 * factorin / gearratio); - motor2offset = (counts2 * factorin / gearratio); + counts1offset = counts1af ; + counts2offset = counts2af; // State transition guard if ( button2_pressed ) { @@ -314,7 +320,7 @@ // State transition guard if ( button2_pressed ) { button2_pressed = false; - motor_curr_state = motor_encoderset; + motor_curr_state = motor_bewegen; motor_state_changed = true; // More functions } @@ -389,9 +395,9 @@ button2.fall(&button2Press); while (true) { - pc.printf("Omega1: %f Omega 2: %f controlsignal1: %f \r\n", omega1, omega2, controlsignal1); - pc.printf("Currentstate: %i\r\n",motor_curr_state); - pc.printf("motor1offset: %f motor2offset: %f\r\n",motor1offset,motor2offset); + pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1); + pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2); + pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle); wait(0.5); } }