mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 21:394a7a1deb73
- Parent:
- 20:ac1b4ffa3323
- Child:
- 22:6cc93216b323
diff -r ac1b4ffa3323 -r 394a7a1deb73 main.cpp --- a/main.cpp Fri Oct 04 13:28:00 2019 +0000 +++ b/main.cpp Mon Oct 14 08:50:26 2019 +0000 @@ -5,121 +5,248 @@ #include "HIDScope.h" #include "MODSERIAL.h" -// ENC1A --> D13 -// ENC1B --> D12 -// POT1 --> A0 -// LED1 --> D3 -// BUT1 --> D1 -// BUT2 --> D0 - -MODSERIAL pc(USBTX, USBRX); -Ticker ControlMotor1; - -AnalogIn theta_ref(A0); -DigitalOut PWM_motor_1(D6); +// Pins +MODSERIAL pc(USBTX, USBRX); +InterruptIn stepresponse(D0); +FastPWM PWM_motor_1(D6); DigitalOut direction_motor_1(D7); - -HIDScope scope(2); -QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING); -Ticker RW_scope; +InterruptIn button_1(SW2); +InterruptIn button_2(SW3); // variables -float duty_cycle_motor_1; -volatile int on_time_ms; // The time the LED should be on, in microseconds -volatile int off_time_ms; - -float theta = 0; -volatile double x_0; -volatile double x_prev_0 = 0; -volatile double y_0; -volatile double x_1; -volatile double x_prev_1 = 0; -volatile double y_1; +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; +float theta_error; +volatile float theta_reference; +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; -float omega; -float torque; -float torque_p; -float torque_i; -float torque_d; -float velocity; -float theta_error; +// functies +float CalculateError() +{ + theta_error = theta_reference-theta; + return theta_error; +} -float Kp = 5; -float Ki = 3; -float Kd = 3; -float Ts = 1; - -// functions -float CalculateError(){ - //theta_error = (360*theta_ref.read())-theta; - theta_error = 180; - 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); +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; + 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; - //float torque_d = Kd*error_derivative; - //error_prev = theta_error; + 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_p;} + float torque = torque_p + torque_i + torque_d; + return torque; +} -void CalculateDirectionMotor1() { - if (theta_error >= 0) { - direction_motor_1 = 1;} - else { - direction_motor_1 = 0;}} +void CalculateDirectionMotor1() +{ + direction_motor_1 = Controller(CalculateError()) <= 0.0f ; +} -//float CalculateVelocityMotor1() { -// return theta_error/Ts;} +void WriteScope() +{ + scope.set(0,theta); + //scope.set(1,Controller(CalculateError())); + scope.set(1,CalculateError()); + scope.set(2,theta_reference); + scope.send(); +} -void PulseWidthModulation() // Bepaalt de duty cycle gebaseerd op de potentiometer en zet de motor dan op die snelheid mbh PWM -{ on_time_ms = (int) (duty_cycle_motor_1*(1/1e2)*1e3); // Deze variable maken - off_time_ms = (int) ((1-duty_cycle_motor_1)*(1/1e2)*1e3); - PWM_motor_1 = 1; - wait_ms(on_time_ms); - PWM_motor_1 = 0; - wait_ms(off_time_ms);} - -void MotorDirection(){ +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. } -void ReadEncoderAndWriteScope(){ - theta = ((360/64)*encoder.getPulses())/131.25; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360. - x_0 = theta; - y_0 = (x_prev_0 + x_0)/2.0; - scope.set(0,y_0); - x_prev_0 = x_0; - y_1 = (x_0 - x_prev_0)/Ts; - scope.set(1,y_1); - scope.send();} +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)); +} + +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 doeietsnuttigs(){ - // Controller(CalculateError());} - +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() -{ +int main(){ pc.baud(115200); pc.printf("Hello World!\n\r"); - //ControlMotor1.attach(&ReadEncoderAndWriteScope, Ts); - //ControlMotor1.attach(&doeietsnuttigs, Ts); - while(1) { - pc.printf("%f \n\r",Controller(CalculateError())); - wait(1); - } + 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(); + } } \ No newline at end of file