mag niet van hendrik D:
Dependencies: mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM
main.cpp@20:ac1b4ffa3323, 2019-10-04 (annotated)
- Committer:
- Hendrikvg
- Date:
- Fri Oct 04 13:28:00 2019 +0000
- Revision:
- 20:ac1b4ffa3323
- Parent:
- 19:5ac8b7af77a3
- Child:
- 21:394a7a1deb73
torque_p van de controller werkt, verder werkt er nog niets.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hendrikvg | 17:cacf9e75eda7 | 1 | #include "QEI.h" |
Hendrikvg | 15:80b3ac2b8448 | 2 | #include "mbed.h" |
Hendrikvg | 20:ac1b4ffa3323 | 3 | #include "BiQuad.h" |
Hendrikvg | 14:20f11bb58244 | 4 | #include "FastPWM.h" |
Hendrikvg | 17:cacf9e75eda7 | 5 | #include "HIDScope.h" |
Hendrikvg | 16:40183eeadb6d | 6 | #include "MODSERIAL.h" |
Hendrikvg | 9:12b9865e7373 | 7 | |
Hendrikvg | 18:50c04dd523ea | 8 | // ENC1A --> D13 |
Hendrikvg | 18:50c04dd523ea | 9 | // ENC1B --> D12 |
Hendrikvg | 18:50c04dd523ea | 10 | // POT1 --> A0 |
Hendrikvg | 18:50c04dd523ea | 11 | // LED1 --> D3 |
Hendrikvg | 18:50c04dd523ea | 12 | // BUT1 --> D1 |
Hendrikvg | 18:50c04dd523ea | 13 | // BUT2 --> D0 |
Hendrikvg | 18:50c04dd523ea | 14 | |
Hendrikvg | 20:ac1b4ffa3323 | 15 | MODSERIAL pc(USBTX, USBRX); |
Hendrikvg | 20:ac1b4ffa3323 | 16 | Ticker ControlMotor1; |
Hendrikvg | 15:80b3ac2b8448 | 17 | |
Hendrikvg | 20:ac1b4ffa3323 | 18 | AnalogIn theta_ref(A0); |
Hendrikvg | 20:ac1b4ffa3323 | 19 | DigitalOut PWM_motor_1(D6); |
Hendrikvg | 20:ac1b4ffa3323 | 20 | DigitalOut direction_motor_1(D7); |
RobertoO | 0:67c50348f842 | 21 | |
Hendrikvg | 17:cacf9e75eda7 | 22 | HIDScope scope(2); |
Hendrikvg | 17:cacf9e75eda7 | 23 | QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING); |
Hendrikvg | 17:cacf9e75eda7 | 24 | Ticker RW_scope; |
Hendrikvg | 17:cacf9e75eda7 | 25 | |
Hendrikvg | 15:80b3ac2b8448 | 26 | // variables |
Hendrikvg | 20:ac1b4ffa3323 | 27 | float duty_cycle_motor_1; |
Hendrikvg | 16:40183eeadb6d | 28 | volatile int on_time_ms; // The time the LED should be on, in microseconds |
Hendrikvg | 16:40183eeadb6d | 29 | volatile int off_time_ms; |
Hendrikvg | 15:80b3ac2b8448 | 30 | |
Hendrikvg | 20:ac1b4ffa3323 | 31 | float theta = 0; |
Hendrikvg | 20:ac1b4ffa3323 | 32 | volatile double x_0; |
Hendrikvg | 20:ac1b4ffa3323 | 33 | volatile double x_prev_0 = 0; |
Hendrikvg | 20:ac1b4ffa3323 | 34 | volatile double y_0; |
Hendrikvg | 20:ac1b4ffa3323 | 35 | volatile double x_1; |
Hendrikvg | 20:ac1b4ffa3323 | 36 | volatile double x_prev_1 = 0; |
Hendrikvg | 20:ac1b4ffa3323 | 37 | volatile double y_1; |
Hendrikvg | 20:ac1b4ffa3323 | 38 | |
Hendrikvg | 20:ac1b4ffa3323 | 39 | float omega; |
Hendrikvg | 20:ac1b4ffa3323 | 40 | float torque; |
Hendrikvg | 20:ac1b4ffa3323 | 41 | float torque_p; |
Hendrikvg | 20:ac1b4ffa3323 | 42 | float torque_i; |
Hendrikvg | 20:ac1b4ffa3323 | 43 | float torque_d; |
Hendrikvg | 20:ac1b4ffa3323 | 44 | float velocity; |
Hendrikvg | 20:ac1b4ffa3323 | 45 | float theta_error; |
Hendrikvg | 20:ac1b4ffa3323 | 46 | |
Hendrikvg | 20:ac1b4ffa3323 | 47 | float Kp = 5; |
Hendrikvg | 20:ac1b4ffa3323 | 48 | float Ki = 3; |
Hendrikvg | 20:ac1b4ffa3323 | 49 | float Kd = 3; |
Hendrikvg | 20:ac1b4ffa3323 | 50 | float Ts = 1; |
Hendrikvg | 17:cacf9e75eda7 | 51 | |
Hendrikvg | 15:80b3ac2b8448 | 52 | // functions |
Hendrikvg | 20:ac1b4ffa3323 | 53 | float CalculateError(){ |
Hendrikvg | 20:ac1b4ffa3323 | 54 | //theta_error = (360*theta_ref.read())-theta; |
Hendrikvg | 20:ac1b4ffa3323 | 55 | theta_error = 180; |
Hendrikvg | 20:ac1b4ffa3323 | 56 | return theta_error;} |
Hendrikvg | 16:40183eeadb6d | 57 | |
Hendrikvg | 20:ac1b4ffa3323 | 58 | float Controller(float theta_error){ |
Hendrikvg | 20:ac1b4ffa3323 | 59 | //static float error_integral = 0; |
Hendrikvg | 20:ac1b4ffa3323 | 60 | //static float error_prev = 0; |
Hendrikvg | 20:ac1b4ffa3323 | 61 | //static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Hendrikvg | 20:ac1b4ffa3323 | 62 | |
Hendrikvg | 20:ac1b4ffa3323 | 63 | // Proportional part: |
Hendrikvg | 20:ac1b4ffa3323 | 64 | float torque_p = Kp * theta_error; |
Hendrikvg | 20:ac1b4ffa3323 | 65 | |
Hendrikvg | 20:ac1b4ffa3323 | 66 | // Integral part: |
Hendrikvg | 20:ac1b4ffa3323 | 67 | //error_integral = error_integral + theta_error * Ts; |
Hendrikvg | 20:ac1b4ffa3323 | 68 | //float torque_i = Ki * error_integral; |
Hendrikvg | 20:ac1b4ffa3323 | 69 | |
Hendrikvg | 20:ac1b4ffa3323 | 70 | // Derivative part: |
Hendrikvg | 20:ac1b4ffa3323 | 71 | //float error_derivative = (theta_error - error_prev)/Ts; |
Hendrikvg | 20:ac1b4ffa3323 | 72 | //float filtered_error_derivative = LowPassFilter.step(error_derivative); |
Hendrikvg | 20:ac1b4ffa3323 | 73 | //float torque_d = Kd * filtered_error_derivative; |
Hendrikvg | 20:ac1b4ffa3323 | 74 | //float torque_d = Kd*error_derivative; |
Hendrikvg | 20:ac1b4ffa3323 | 75 | //error_prev = theta_error; |
Hendrikvg | 20:ac1b4ffa3323 | 76 | |
Hendrikvg | 20:ac1b4ffa3323 | 77 | // Sum all parts and return it |
Hendrikvg | 20:ac1b4ffa3323 | 78 | //float torque = torque_p + torque_i + torque_d; |
Hendrikvg | 20:ac1b4ffa3323 | 79 | return torque_p;} |
Hendrikvg | 16:40183eeadb6d | 80 | |
Hendrikvg | 20:ac1b4ffa3323 | 81 | void CalculateDirectionMotor1() { |
Hendrikvg | 20:ac1b4ffa3323 | 82 | if (theta_error >= 0) { |
Hendrikvg | 20:ac1b4ffa3323 | 83 | direction_motor_1 = 1;} |
Hendrikvg | 20:ac1b4ffa3323 | 84 | else { |
Hendrikvg | 20:ac1b4ffa3323 | 85 | direction_motor_1 = 0;}} |
Hendrikvg | 20:ac1b4ffa3323 | 86 | |
Hendrikvg | 20:ac1b4ffa3323 | 87 | //float CalculateVelocityMotor1() { |
Hendrikvg | 20:ac1b4ffa3323 | 88 | // return theta_error/Ts;} |
Hendrikvg | 16:40183eeadb6d | 89 | |
Hendrikvg | 16:40183eeadb6d | 90 | void PulseWidthModulation() // Bepaalt de duty cycle gebaseerd op de potentiometer en zet de motor dan op die snelheid mbh PWM |
Hendrikvg | 20:ac1b4ffa3323 | 91 | { on_time_ms = (int) (duty_cycle_motor_1*(1/1e2)*1e3); // Deze variable maken |
Hendrikvg | 20:ac1b4ffa3323 | 92 | off_time_ms = (int) ((1-duty_cycle_motor_1)*(1/1e2)*1e3); |
Hendrikvg | 19:5ac8b7af77a3 | 93 | PWM_motor_1 = 1; |
Hendrikvg | 16:40183eeadb6d | 94 | wait_ms(on_time_ms); |
Hendrikvg | 19:5ac8b7af77a3 | 95 | PWM_motor_1 = 0; |
Hendrikvg | 19:5ac8b7af77a3 | 96 | wait_ms(off_time_ms);} |
Hendrikvg | 20:ac1b4ffa3323 | 97 | |
Hendrikvg | 20:ac1b4ffa3323 | 98 | void MotorDirection(){ |
Hendrikvg | 20:ac1b4ffa3323 | 99 | } |
Hendrikvg | 16:40183eeadb6d | 100 | |
Hendrikvg | 19:5ac8b7af77a3 | 101 | void ReadEncoderAndWriteScope(){ |
Hendrikvg | 20:ac1b4ffa3323 | 102 | 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. |
Hendrikvg | 20:ac1b4ffa3323 | 103 | x_0 = theta; |
Hendrikvg | 20:ac1b4ffa3323 | 104 | y_0 = (x_prev_0 + x_0)/2.0; |
Hendrikvg | 20:ac1b4ffa3323 | 105 | scope.set(0,y_0); |
Hendrikvg | 20:ac1b4ffa3323 | 106 | x_prev_0 = x_0; |
Hendrikvg | 20:ac1b4ffa3323 | 107 | y_1 = (x_0 - x_prev_0)/Ts; |
Hendrikvg | 20:ac1b4ffa3323 | 108 | scope.set(1,y_1); |
Hendrikvg | 19:5ac8b7af77a3 | 109 | scope.send();} |
Hendrikvg | 17:cacf9e75eda7 | 110 | |
Hendrikvg | 20:ac1b4ffa3323 | 111 | //void doeietsnuttigs(){ |
Hendrikvg | 20:ac1b4ffa3323 | 112 | // Controller(CalculateError());} |
Hendrikvg | 20:ac1b4ffa3323 | 113 | |
Hendrikvg | 15:80b3ac2b8448 | 114 | // main |
Hendrikvg | 15:80b3ac2b8448 | 115 | int main() |
Hendrikvg | 15:80b3ac2b8448 | 116 | { |
RobertoO | 0:67c50348f842 | 117 | pc.baud(115200); |
Hendrikvg | 17:cacf9e75eda7 | 118 | pc.printf("Hello World!\n\r"); |
Hendrikvg | 20:ac1b4ffa3323 | 119 | //ControlMotor1.attach(&ReadEncoderAndWriteScope, Ts); |
Hendrikvg | 20:ac1b4ffa3323 | 120 | //ControlMotor1.attach(&doeietsnuttigs, Ts); |
Hendrikvg | 20:ac1b4ffa3323 | 121 | while(1) { |
Hendrikvg | 20:ac1b4ffa3323 | 122 | pc.printf("%f \n\r",Controller(CalculateError())); |
Hendrikvg | 20:ac1b4ffa3323 | 123 | wait(1); |
Hendrikvg | 9:12b9865e7373 | 124 | } |
Hendrikvg | 2:d9b0ebf3fcca | 125 | } |