mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 22:6cc93216b323
- Parent:
- 21:394a7a1deb73
- Child:
- 23:78898ddfb103
--- a/main.cpp Mon Oct 14 08:50:26 2019 +0000 +++ b/main.cpp Mon Oct 14 13:26:46 2019 +0000 @@ -26,9 +26,9 @@ bool next = false; HIDScope scope(3); QEI encoder(D12,D13,NC,8400,QEI::X4_ENCODING); -volatile double theta; -float theta_error; -volatile float theta_reference; +volatile double theta_1; +volatile float theta_error_1; +volatile float theta_reference_1; float K = 1; float ti = 0.1; float td = 10; @@ -38,9 +38,9 @@ float Ts = 0.001; // functies -float CalculateError() +float CalculateError(float theta_reference,float theta) { - theta_error = theta_reference-theta; + float theta_error = theta_reference-theta; return theta_error; } @@ -68,29 +68,30 @@ return torque; } -void CalculateDirectionMotor1() +void CalculateDirectionMotor() { - direction_motor_1 = Controller(CalculateError()) <= 0.0f ; + direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1)) <= 0.0f; + //direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2)) <= 0.0f; } void WriteScope() { - scope.set(0,theta); + scope.set(0,theta_1); //scope.set(1,Controller(CalculateError())); - scope.set(1,CalculateError()); - scope.set(2,theta_reference); + scope.set(1,CalculateError(theta_reference_1,theta_1)); + scope.set(2,theta_reference_1); scope.send(); } void ReadEncoder(){ - theta = ((360.0f/64.0f)*(float)encoder.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. + theta_1 = ((360.0f/64.0f)*(float)encoder.getPulses())/131.25f; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. } void Motor_1() { ReadEncoder(); - theta_reference = 360*sin(0.1f*sinus_time.read()*2*3.14f); - CalculateDirectionMotor1(); - PWM_motor_1.write(fabs(Controller(CalculateError())/360.0f)); + theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); + CalculateDirectionMotor(); + PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1))/360.0f)); } void go_next_1()