Basis voor de pid controller van de arm.
Dependencies: MODSERIAL QEI mbed
main.cpp@0:67f1f06bdf17, 2016-10-21 (annotated)
- Committer:
- dirknefkens
- Date:
- Fri Oct 21 11:21:29 2016 +0000
- Revision:
- 0:67f1f06bdf17
Begin PID controller voor arm
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
dirknefkens | 0:67f1f06bdf17 | 1 | #include "mbed.h" |
dirknefkens | 0:67f1f06bdf17 | 2 | #include "MODSERIAL.h" |
dirknefkens | 0:67f1f06bdf17 | 3 | #include "QEI.h" |
dirknefkens | 0:67f1f06bdf17 | 4 | |
dirknefkens | 0:67f1f06bdf17 | 5 | |
dirknefkens | 0:67f1f06bdf17 | 6 | MODSERIAL pc(USBTX, USBRX); |
dirknefkens | 0:67f1f06bdf17 | 7 | DigitalOut motor_horizontal_dir(D7); //Moet nog aangepast worden aan de motoren. |
dirknefkens | 0:67f1f06bdf17 | 8 | PwmOut motor_horizontal_pwm(D6); |
dirknefkens | 0:67f1f06bdf17 | 9 | DigitalOut motor_vertical_dir(D10); //Moet nog aangepast worden aan de motoren. |
dirknefkens | 0:67f1f06bdf17 | 10 | PwmOut motor_vertical_pwm(D11); |
dirknefkens | 0:67f1f06bdf17 | 11 | |
dirknefkens | 0:67f1f06bdf17 | 12 | QEI Encoder_hor(D13,D12,NC,8400); //For the encoder |
dirknefkens | 0:67f1f06bdf17 | 13 | QEI Encoder_ver(*,*,NC,8400); |
dirknefkens | 0:67f1f06bdf17 | 14 | int Pulse_state_hor; |
dirknefkens | 0:67f1f06bdf17 | 15 | int Previous_pulse_state_hor; |
dirknefkens | 0:67f1f06bdf17 | 16 | double Pulse_destination_hor = 0; |
dirknefkens | 0:67f1f06bdf17 | 17 | int Pulse_state_ver; |
dirknefkens | 0:67f1f06bdf17 | 18 | int Previous_pulse_state_ver; |
dirknefkens | 0:67f1f06bdf17 | 19 | double Pulse_destination_ver = 0; |
dirknefkens | 0:67f1f06bdf17 | 20 | |
dirknefkens | 0:67f1f06bdf17 | 21 | DigitalIn Button1(SW2); //Buttons voor testen in plaats van elektrodes. |
dirknefkens | 0:67f1f06bdf17 | 22 | DigitalIn Button2(SW3); |
dirknefkens | 0:67f1f06bdf17 | 23 | |
dirknefkens | 0:67f1f06bdf17 | 24 | float Elektrode_1; //Niet nodig voor de test, input vanuit arm. |
dirknefkens | 0:67f1f06bdf17 | 25 | float Elektrode_2; |
dirknefkens | 0:67f1f06bdf17 | 26 | bool Elektrode_3; |
dirknefkens | 0:67f1f06bdf17 | 27 | |
dirknefkens | 0:67f1f06bdf17 | 28 | float Wait_time = 0.02f; |
dirknefkens | 0:67f1f06bdf17 | 29 | float Sensitivity = 10; |
dirknefkens | 0:67f1f06bdf17 | 30 | bool Control_active; |
dirknefkens | 0:67f1f06bdf17 | 31 | int Arm_state = 1; |
dirknefkens | 0:67f1f06bdf17 | 32 | |
dirknefkens | 0:67f1f06bdf17 | 33 | float P = 1; //Goede waardes vinden. |
dirknefkens | 0:67f1f06bdf17 | 34 | float I = 0.01; |
dirknefkens | 0:67f1f06bdf17 | 35 | float D = 0.05; |
dirknefkens | 0:67f1f06bdf17 | 36 | double Integrated_distance = 0; |
dirknefkens | 0:67f1f06bdf17 | 37 | double PID_value = 0; |
dirknefkens | 0:67f1f06bdf17 | 38 | |
dirknefkens | 0:67f1f06bdf17 | 39 | Ticker Motor_control; //Testen of er niks afgekapt wordt en voor een goede tijd. |
dirknefkens | 0:67f1f06bdf17 | 40 | |
dirknefkens | 0:67f1f06bdf17 | 41 | void Control_activator{ |
dirknefkens | 0:67f1f06bdf17 | 42 | Control_active = true; |
dirknefkens | 0:67f1f06bdf17 | 43 | } |
dirknefkens | 0:67f1f06bdf17 | 44 | |
dirknefkens | 0:67f1f06bdf17 | 45 | double Pid_controller(int Pulse_state,int Previous_pulse_state,double Pulse_destination) { |
dirknefkens | 0:67f1f06bdf17 | 46 | Integrated_distance = Integrated_distance + (Pulse_destination - Pulse_state) * Wait_time; |
dirknefkens | 0:67f1f06bdf17 | 47 | PID_value = (P * (Pulse_destination - Pulse_state) + I * Integrated_distance + D * (Previous_pulse_state - Pulse_state)/Wait_time)/500; |
dirknefkens | 0:67f1f06bdf17 | 48 | return PID_value; |
dirknefkens | 0:67f1f06bdf17 | 49 | } |
dirknefkens | 0:67f1f06bdf17 | 50 | |
dirknefkens | 0:67f1f06bdf17 | 51 | void setmotor1(double val){ |
dirknefkens | 0:67f1f06bdf17 | 52 | motor_horizontal_dir.write(val>=0?1:0); |
dirknefkens | 0:67f1f06bdf17 | 53 | motor_horizontal_pwm.write(fabs(val)>1;1.0:fabs(val)); |
dirknefkens | 0:67f1f06bdf17 | 54 | } |
dirknefkens | 0:67f1f06bdf17 | 55 | |
dirknefkens | 0:67f1f06bdf17 | 56 | void setmotor2(double val){ |
dirknefkens | 0:67f1f06bdf17 | 57 | motor_vertical_dir.write(val>=0?1:0); |
dirknefkens | 0:67f1f06bdf17 | 58 | motor_vertical_pwm.write(fabs(val)>1;1.0:fabs(val)); |
dirknefkens | 0:67f1f06bdf17 | 59 | } |
dirknefkens | 0:67f1f06bdf17 | 60 | |
dirknefkens | 0:67f1f06bdf17 | 61 | int main() |
dirknefkens | 0:67f1f06bdf17 | 62 | { |
dirknefkens | 0:67f1f06bdf17 | 63 | pc.baud(115200); |
dirknefkens | 0:67f1f06bdf17 | 64 | Motor_control.attach(&Control_activator,Wait_time); |
dirknefkens | 0:67f1f06bdf17 | 65 | while (true) { |
dirknefkens | 0:67f1f06bdf17 | 66 | Previous_pulse_state_hor = Pulse_state_hor; |
dirknefkens | 0:67f1f06bdf17 | 67 | Pulse_state_hor = Encoder_hor.getPulses(); |
dirknefkens | 0:67f1f06bdf17 | 68 | Previous_pulse_state_ver = Pulse_state_ver; |
dirknefkens | 0:67f1f06bdf17 | 69 | Pulse_state_ver = Encoder_ver.getPulses(); |
dirknefkens | 0:67f1f06bdf17 | 70 | if Control_active(){ |
dirknefkens | 0:67f1f06bdf17 | 71 | switch Arm_state{ |
dirknefkens | 0:67f1f06bdf17 | 72 | case "1" |
dirknefkens | 0:67f1f06bdf17 | 73 | |
dirknefkens | 0:67f1f06bdf17 | 74 | break |
dirknefkens | 0:67f1f06bdf17 | 75 | case "2" |
dirknefkens | 0:67f1f06bdf17 | 76 | |
dirknefkens | 0:67f1f06bdf17 | 77 | break |
dirknefkens | 0:67f1f06bdf17 | 78 | case "3" |
dirknefkens | 0:67f1f06bdf17 | 79 | |
dirknefkens | 0:67f1f06bdf17 | 80 | break |
dirknefkens | 0:67f1f06bdf17 | 81 | case "4" |
dirknefkens | 0:67f1f06bdf17 | 82 | |
dirknefkens | 0:67f1f06bdf17 | 83 | break |
dirknefkens | 0:67f1f06bdf17 | 84 | double PID_value_ver = PID_controller(Pulse_state_ver,Previous_pulse_state_ver,Pulse_destination_ver); |
dirknefkens | 0:67f1f06bdf17 | 85 | setmotor2(PID_value_hor); |
dirknefkens | 0:67f1f06bdf17 | 86 | double PID_value_hor = PID_controller(Pulse_state_hor,Previous_pulse_state_hor,Pulse_destination_hor); |
dirknefkens | 0:67f1f06bdf17 | 87 | setmotor1(PID_value_hor); |
dirknefkens | 0:67f1f06bdf17 | 88 | Control_active = false; |
dirknefkens | 0:67f1f06bdf17 | 89 | } |
dirknefkens | 0:67f1f06bdf17 | 90 | } |
dirknefkens | 0:67f1f06bdf17 | 91 | wait(0.01f); |
dirknefkens | 0:67f1f06bdf17 | 92 | } |
dirknefkens | 0:67f1f06bdf17 | 93 | } |