lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 23:78898ddfb103
- Parent:
- 22:6cc93216b323
- Child:
- 24:a9ec9b836fd9
--- a/main.cpp Mon Oct 14 13:26:46 2019 +0000 +++ b/main.cpp Mon Oct 21 10:14:44 2019 +0000 @@ -9,14 +9,16 @@ MODSERIAL pc(USBTX, USBRX); InterruptIn stepresponse(D0); FastPWM PWM_motor_1(D6); +FastPWM PWM_motor_2(D5); DigitalOut direction_motor_1(D7); +DigitalOut direction_motor_2(D4); InterruptIn button_1(SW2); InterruptIn button_2(SW3); // variables Ticker TickerStateMachine; -Ticker ControlMotor1; -Ticker RW_scope; +Ticker motor_control; +Ticker write_scope; Timer sinus_time; enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement}; states CurrentState = start; @@ -25,17 +27,18 @@ bool emg = false; bool next = false; HIDScope scope(3); -QEI encoder(D12,D13,NC,8400,QEI::X4_ENCODING); +QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING); +QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING); volatile double theta_1; -volatile float theta_error_1; +//volatile float theta_error_1; volatile float theta_reference_1; -float K = 1; -float ti = 0.1; -float td = 10; -float Kp = K*(1+td/ti); -float Ki = K/ti; -float Kd = K*td; +volatile double theta_2; +//volatile float theta_error_2; +volatile float theta_reference_2; float Ts = 0.001; +float Kp; +float Ki; +float Kd; // functies float CalculateError(float theta_reference,float theta) @@ -44,8 +47,26 @@ return theta_error; } -float Controller(float theta_error) +float Controller(float theta_error, bool motor) { + if (motor == false) + { + float K = 1; + float ti = 0.1; + float td = 10; + Kp = K*(1+td/ti); + Ki = K/ti; + Kd = K*td; + } + else + { + float K = 1; + float ti = 0.1; + float td = 10; + Kp = K*(1+td/ti); + Ki = K/ti; + Kd = K*td; + } static float error_integral = 0; static float error_prev = 0; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); @@ -70,28 +91,31 @@ void CalculateDirectionMotor() { - direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1)) <= 0.0f; - //direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2)) <= 0.0f; + direction_motor_1 = Controller(CalculateError(theta_reference_1,theta_1),0) <= 0.0f; + direction_motor_2 = Controller(CalculateError(theta_reference_2,theta_2),1) <= 0.0f; } void WriteScope() { scope.set(0,theta_1); - //scope.set(1,Controller(CalculateError())); scope.set(1,CalculateError(theta_reference_1,theta_1)); scope.set(2,theta_reference_1); scope.send(); } -void ReadEncoder(){ - 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 ReadEncoder() +{ + theta_1 = ((360.0f/64.0f)*(float)encoder_1.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_2 = ((360.0f/64.0f)*(float)encoder_2.getPulses())/131.25f; +} -void Motor_1() { +void MotorControl() +{ ReadEncoder(); - theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); + theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript CalculateDirectionMotor(); - PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1))/360.0f)); + PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f)); + PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f)); } void go_next_1() @@ -244,8 +268,8 @@ button_2.fall(go_next_2); sinus_time.start(); PWM_motor_1.period_ms(10); - ControlMotor1.attach(&Motor_1, Ts); - RW_scope.attach(&WriteScope, 0.1); + motor_control.attach(&MotorControl, Ts); + write_scope.attach(&WriteScope, 0.1); //TickerStateMachine.attach(StateMachine,1.00f); while(true) { StateMachine();