mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- Hendrikvg
- Date:
- 2019-10-14
- Revision:
- 22:6cc93216b323
- Parent:
- 21:394a7a1deb73
- Child:
- 23:78898ddfb103
File content as of revision 22:6cc93216b323:
#include "QEI.h" #include "mbed.h" #include "BiQuad.h" #include "FastPWM.h" #include "HIDScope.h" #include "MODSERIAL.h" // Pins MODSERIAL pc(USBTX, USBRX); InterruptIn stepresponse(D0); FastPWM PWM_motor_1(D6); DigitalOut direction_motor_1(D7); InterruptIn button_1(SW2); InterruptIn button_2(SW3); // variables Ticker TickerStateMachine; Ticker ControlMotor1; Ticker RW_scope; Timer sinus_time; enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement}; states CurrentState = start; bool StateChanged = true; bool demo = false; bool emg = false; bool next = false; HIDScope scope(3); QEI encoder(D12,D13,NC,8400,QEI::X4_ENCODING); volatile double theta_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; float Ts = 0.001; // functies float CalculateError(float theta_reference,float theta) { float theta_error = theta_reference-theta; return theta_error; } float Controller(float theta_error) { static float error_integral = 0; static float error_prev = 0; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: float torque_p = Kp * theta_error; // Integral part: error_integral = error_integral + theta_error * Ts; float torque_i = Ki * error_integral; // Derivative part: float error_derivative = (theta_error - error_prev)/Ts; float filtered_error_derivative = LowPassFilter.step(error_derivative); float torque_d = Kd * filtered_error_derivative; error_prev = theta_error; // Sum all parts and return it float torque = torque_p + torque_i + torque_d; return torque; } 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; } 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 Motor_1() { ReadEncoder(); 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() { demo = !demo; } void go_next_2() { emg = !emg; next = emg; } void while_start() { // Do startup stuff CurrentState = motor_calibration; StateChanged = true; } void while_motor_calibration() { // Do motor calibration stuff if (demo) // bool aanmaken voor demo (switch oid aanmaken) { CurrentState = demo_mode; StateChanged = true; } if (emg) // bool aanmaken voor EMG (switch oid aanmaken) { CurrentState = emg_calibration; StateChanged = true; } } void while_demo_mode() { // Do demo mode stuff if (!demo) // bool aanmaken voor demo (switch oid) { emg = true; } if (emg) // bool aanmaken voor EMG (switch oid aanmaken) { CurrentState = emg_calibration; StateChanged = true; } } void while_emg_calibration() { // Do emg calibration stuff if (!emg) // bool aanmaken voor EMG (switch) { CurrentState = vertical_movement; StateChanged = true; } } void while_vertical_movement() { // Do vertical movement stuff if (next) // EMG gebaseerde threshold aanmaken { CurrentState = horizontal_movement; StateChanged = true; } } void while_horizontal_movement() { // Do horizontal movement stuff if (!next) // EMG gebaseerde threshold aanmaken { CurrentState = vertical_movement; StateChanged = true; } } void StateTransition() { if (StateChanged) { if (CurrentState == start) { pc.printf("Initiating start.\n\r"); } if (CurrentState == motor_calibration) { pc.printf("Initiating motor_calibration.\n\r"); } if (CurrentState == demo_mode) { pc.printf("Initiating demo_mode.\n\r"); } if (CurrentState == emg_calibration) { pc.printf("Initiating emg_calibration.\n\r"); } if (CurrentState == vertical_movement) { pc.printf("Initiating vertical_movement.\n\r"); } if (CurrentState == horizontal_movement) { pc.printf("Initiating horizontal_movement.\n\r"); } StateChanged = false; } } void StateMachine() { switch(CurrentState) { case start: StateTransition(); while_start(); break; case motor_calibration: StateTransition(); while_motor_calibration(); break; case demo_mode: StateTransition(); while_demo_mode(); break; case emg_calibration: StateTransition(); while_emg_calibration(); break; case vertical_movement: StateTransition(); while_vertical_movement(); break; case horizontal_movement: StateTransition(); while_horizontal_movement(); break; default: pc.printf("Unknown or unimplemented state reached!\n\r"); } } // main int main(){ pc.baud(115200); pc.printf("Hello World!\n\r"); button_1.fall(go_next_1); 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); //TickerStateMachine.attach(StateMachine,1.00f); while(true) { StateMachine(); } }