Dirk Nefkens / Mbed 2 deprecated PID_controller_grijparm

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?

UserRevisionLine numberNew 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 }