sibren vuurberg / Mbed 2 deprecated PID_controler

PID_controler voor het latere script

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@4:61d9325d7f2e, 2016-11-03 (annotated)

Committer:
sivuu
Date:
Thu Nov 03 14:37:04 2016 +0000
Revision:
4:61d9325d7f2e
Parent:
3:56f31420abac 
nu hopelijk met twee PID controllers en een werkende switch

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sivuu 0:57aa29917d4c 1 #include "mbed.h"
sivuu 0:57aa29917d4c 2 #include "QEI.h" //bieb voor encoderfuncties in c++
sivuu 0:57aa29917d4c 3 #include "MODSERIAL.h" //bieb voor modserial
sivuu 0:57aa29917d4c 4 #include "BiQuad.h"
sivuu 1:faa90344cdd8 5 #include "HIDScope.h"
sivuu 1:faa90344cdd8 6 #include "math.h"
sivuu 0:57aa29917d4c 7
sivuu 4:61d9325d7f2e 8 InterruptIn sw3(SW3);
sivuu 0:57aa29917d4c 9 DigitalIn sw2(SW2);
sivuu 0:57aa29917d4c 10 DigitalIn encoder1A(D13);
sivuu 0:57aa29917d4c 11 DigitalIn encoder1B(D12);
sivuu 0:57aa29917d4c 12 DigitalIn encoder2A(D11);
sivuu 0:57aa29917d4c 13 DigitalIn encoder2B(D10);
sivuu 0:57aa29917d4c 14 DigitalIn button_cw(D9);
sivuu 0:57aa29917d4c 15 DigitalIn button_ccw(PTC12);
sivuu 0:57aa29917d4c 16 MODSERIAL pc(USBTX, USBRX);
sivuu 0:57aa29917d4c 17 DigitalOut richting_motor1(D4);
sivuu 0:57aa29917d4c 18 PwmOut pwm_motor1(D5);
sivuu 0:57aa29917d4c 19 DigitalOut richting_motor2(D7);
sivuu 0:57aa29917d4c 20 PwmOut pwm_motor2(D6);
sivuu 0:57aa29917d4c 21 BiQuad PID_controller;
sivuu 3:56f31420abac 22 BiQuad PID_controller2; // maakt een biquad aan voor PID controler 2
sivuu 2:ca645c585a03 23 //HIDScope Scope(1);
sivuu 0:57aa29917d4c 24
sivuu 0:57aa29917d4c 25 //constanten voor de encoder
sivuu 0:57aa29917d4c 26 const int CW = 2.5; //definitie rechtsom 'lage waarde'
sivuu 0:57aa29917d4c 27 const int CCW =-1; //definitie linksom 'hoge waarde'
sivuu 0:57aa29917d4c 28 const float gearboxratio=131.25; // gearboxratio van encoder naar motor
sivuu 0:57aa29917d4c 29 const float rev_rond=64.0; // aantal revoluties per omgang van de encoder
sivuu 0:57aa29917d4c 30
sivuu 0:57aa29917d4c 31 volatile double current_position_motor1 = 0; // current position is 0 op het begin
sivuu 0:57aa29917d4c 32 volatile double previous_position_motor1 = 0; // previous position is 0 op het begin
sivuu 0:57aa29917d4c 33 volatile double current_position_motor2 = 0;
sivuu 0:57aa29917d4c 34 volatile double previous_position_motor2 = 0;
sivuu 0:57aa29917d4c 35 volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
sivuu 0:57aa29917d4c 36 volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 0:57aa29917d4c 37 volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 2:ca645c585a03 38 double ticker_TS=0.001; // zorgt voor een tijdstap van de ticker van 0.1 seconde
sivuu 0:57aa29917d4c 39 volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is
sivuu 0:57aa29917d4c 40 Ticker t; // maakt ticker t aan
sivuu 0:57aa29917d4c 41 volatile double value1_resetbutton = 0; // deze value wordt gebruikt zodat als er bij de reset button na het bereiken van de waarde nul. De motor stopt met draaien.
sivuu 0:57aa29917d4c 42 volatile double value2_resetbutton = 0;
sivuu 0:57aa29917d4c 43
sivuu 0:57aa29917d4c 44
sivuu 0:57aa29917d4c 45 volatile float d_ref = 0;
sivuu 3:56f31420abac 46 volatile float d_ref2 = 0;
sivuu 0:57aa29917d4c 47 volatile float d_ref_new;
sivuu 0:57aa29917d4c 48 const float w_ref = 3;
sivuu 3:56f31420abac 49 const float w_ref2 = 4.5;
sivuu 2:ca645c585a03 50 const double Ts = 0.001;
sivuu 2:ca645c585a03 51 const double Kp = 0.3823;
sivuu 2:ca645c585a03 52 const double Ki = 0.127875;
sivuu 2:ca645c585a03 53 const double Kd = 0.2519;
sivuu 0:57aa29917d4c 54 const double N = 100;
sivuu 3:56f31420abac 55 const double Ts2 = 0.001;
sivuu 3:56f31420abac 56 const double Kp2 = 0.3823;
sivuu 3:56f31420abac 57 const double Ki2 = 0.127875;
sivuu 3:56f31420abac 58 const double Kd2 = 0.2519;
sivuu 3:56f31420abac 59 const double N2 = 100;
sivuu 0:57aa29917d4c 60 Ticker tick;
sivuu 3:56f31420abac 61 Ticker controllerticker;
sivuu 3:56f31420abac 62 Ticker tick2; // ticker voor reverence voor motor 2
sivuu 3:56f31420abac 63 Ticker controllerticker2; //ticker voor controler voor motor 2
sivuu 0:57aa29917d4c 64 float rev_counts_motor1_rad;
sivuu 3:56f31420abac 65 float rev_counts_motor2_rad;
sivuu 2:ca645c585a03 66 volatile float voltage_motor1=0.5;
sivuu 3:56f31420abac 67 volatile float voltage_motor2=0.5; // pwm motor 2 aansturing
sivuu 0:57aa29917d4c 68 volatile double error1;
sivuu 3:56f31420abac 69 volatile double error2; // error voor tweede motor
sivuu 3:56f31420abac 70 volatile double ctrlOutput1;
sivuu 3:56f31420abac 71 volatile double ctrlOutput2; // control output voor motor 2
sivuu 2:ca645c585a03 72 volatile double d_ref_const_cw = 0;
sivuu 2:ca645c585a03 73 volatile double d_ref_const_ccw = 0;
sivuu 3:56f31420abac 74 volatile int n = 0;
sivuu 2:ca645c585a03 75
sivuu 3:56f31420abac 76 void SwitchN() { // maakt simpele functie die 1 bij n optelt voor de switch naar motor 2
sivuu 3:56f31420abac 77 n++;
sivuu 3:56f31420abac 78 }
sivuu 3:56f31420abac 79
sivuu 0:57aa29917d4c 80
sivuu 0:57aa29917d4c 81 void reference(){
sivuu 3:56f31420abac 82 if(n%2== 0){
sivuu 3:56f31420abac 83 if (button_cw == 0){
sivuu 3:56f31420abac 84 d_ref = d_ref + w_ref * Ts;
sivuu 3:56f31420abac 85 }
sivuu 3:56f31420abac 86 if (d_ref > 21.36){
sivuu 3:56f31420abac 87 d_ref = 21.36;
sivuu 3:56f31420abac 88 //d_ref_const_cw = 1;
sivuu 3:56f31420abac 89 }
sivuu 3:56f31420abac 90 else{
sivuu 3:56f31420abac 91 d_ref = d_ref;
sivuu 3:56f31420abac 92 }
sivuu 3:56f31420abac 93
sivuu 3:56f31420abac 94 if (button_ccw == 0){
sivuu 3:56f31420abac 95 d_ref = d_ref - w_ref * Ts;
sivuu 3:56f31420abac 96 }
sivuu 3:56f31420abac 97 if (d_ref < -21.36){
sivuu 3:56f31420abac 98 d_ref = -21.36;
sivuu 3:56f31420abac 99
sivuu 3:56f31420abac 100 }
sivuu 3:56f31420abac 101 else{
sivuu 3:56f31420abac 102 d_ref = d_ref;
sivuu 3:56f31420abac 103 }
sivuu 3:56f31420abac 104 } // haakje sluit voor if loop voor n%==0
sivuu 3:56f31420abac 105 }
sivuu 3:56f31420abac 106
sivuu 3:56f31420abac 107 void reference2(){ // maakt reference aan voor motor2
sivuu 3:56f31420abac 108 if(n%2 != 0){
sivuu 3:56f31420abac 109 if (button_cw == 0){
sivuu 3:56f31420abac 110 d_ref2 = d_ref2 + w_ref2 * Ts;
sivuu 2:ca645c585a03 111 }
sivuu 3:56f31420abac 112 if (d_ref2 > 21.36){
sivuu 3:56f31420abac 113 d_ref2 = 21.36;
sivuu 2:ca645c585a03 114 //d_ref_const_cw = 1;
sivuu 2:ca645c585a03 115 }
sivuu 2:ca645c585a03 116 else{
sivuu 3:56f31420abac 117 d_ref2 = d_ref2;
sivuu 0:57aa29917d4c 118 }
sivuu 0:57aa29917d4c 119
sivuu 2:ca645c585a03 120 if (button_ccw == 0){
sivuu 3:56f31420abac 121 d_ref2 = d_ref2 - w_ref2 * Ts;
sivuu 2:ca645c585a03 122 }
sivuu 3:56f31420abac 123 if (d_ref2 < -21.36){
sivuu 3:56f31420abac 124 d_ref2 = -21.36;
sivuu 2:ca645c585a03 125
sivuu 2:ca645c585a03 126 }
sivuu 2:ca645c585a03 127 else{
sivuu 3:56f31420abac 128 d_ref2 = d_ref2;
sivuu 2:ca645c585a03 129 }
sivuu 3:56f31420abac 130 } // haakje sluit voor if loop met n%!=0)
sivuu 2:ca645c585a03 131 }
sivuu 2:ca645c585a03 132
sivuu 0:57aa29917d4c 133 void m1_controller(){
sivuu 0:57aa29917d4c 134 error1 = d_ref-rev_counts_motor1_rad;
sivuu 3:56f31420abac 135 ctrlOutput1 = PID_controller.step(error1);
sivuu 2:ca645c585a03 136 }
sivuu 3:56f31420abac 137 void m2_controller(){ // void voor tweede controller
sivuu 3:56f31420abac 138 error2 = d_ref2-rev_counts_motor2_rad;
sivuu 3:56f31420abac 139 ctrlOutput2 = PID_controller.step(error2);
sivuu 3:56f31420abac 140 }
sivuu 1:faa90344cdd8 141
sivuu 3:56f31420abac 142 //void tickerfunctie() // maakt een ticker functie aan
sivuu 3:56f31420abac 143 //{
sivuu 3:56f31420abac 144 //tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is
sivuu 3:56f31420abac 145 //}
sivuu 0:57aa29917d4c 146
sivuu 0:57aa29917d4c 147
sivuu 0:57aa29917d4c 148 int main()
sivuu 0:57aa29917d4c 149 {
sivuu 0:57aa29917d4c 150 pc.baud(115200);
sivuu 3:56f31420abac 151 QEI Encoder2(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
sivuu 3:56f31420abac 152 QEI Encoder1(D13,D12, NC, rev_rond,QEI::X4_ENCODING); //encoder motor 2
sivuu 2:ca645c585a03 153 int counts_encoder1; //variabele counts aanmaken
sivuu 3:56f31420abac 154 int counts_encoder2; // variabele counts voor motor2
sivuu 0:57aa29917d4c 155 float rev_counts_motor1;
sivuu 3:56f31420abac 156 float rev_counts_motor2; // variabele coutns voor motor2
sivuu 3:56f31420abac 157 //t.attach(&tickerfunctie,ticker_TS);
sivuu 4:61d9325d7f2e 158 sw3.fall(&SwitchN);
sivuu 2:ca645c585a03 159 tick.attach(&reference,Ts);
sivuu 3:56f31420abac 160 tick2.attach(&reference2,Ts); // ticker voor reference2 attach
sivuu 2:ca645c585a03 161 controllerticker.attach(&m1_controller,Ts);
sivuu 3:56f31420abac 162 controllerticker2.attach(&m2_controller,Ts); // ticker voor controller2 attach
sivuu 3:56f31420abac 163
sivuu 3:56f31420abac 164
sivuu 2:ca645c585a03 165 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
sivuu 3:56f31420abac 166 PID_controller2.PIDF(Kp2,Ki2,Kd2,N2,Ts2);
sivuu 0:57aa29917d4c 167
sivuu 0:57aa29917d4c 168 while (true) {
sivuu 2:ca645c585a03 169 // if (button_cw == 0){
sivuu 2:ca645c585a03 170 //if (tickerflag ==1){
sivuu 0:57aa29917d4c 171
sivuu 2:ca645c585a03 172 // richting_motor1 = 1;
sivuu 2:ca645c585a03 173 // pwm_motor1 = voltage_motor1;
sivuu 2:ca645c585a03 174 //pc.printf("pwm_motor is %f\r\n", pwm_motor1);
sivuu 2:ca645c585a03 175
sivuu 2:ca645c585a03 176 counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden
sivuu 3:56f31420abac 177 counts_encoder2 = Encoder2.getPulses();
sivuu 2:ca645c585a03 178 rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond);
sivuu 2:ca645c585a03 179 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718;
sivuu 3:56f31420abac 180 rev_counts_motor2=(float)counts_encoder2/(gearboxratio*rev_rond);
sivuu 3:56f31420abac 181 rev_counts_motor2_rad=rev_counts_motor2*6.28318530718;
sivuu 3:56f31420abac 182 voltage_motor1=ctrlOutput1;
sivuu 3:56f31420abac 183 voltage_motor2=ctrlOutput2;
sivuu 2:ca645c585a03 184 if (voltage_motor1<0){
sivuu 2:ca645c585a03 185 richting_motor1 = 0;
sivuu 2:ca645c585a03 186 }
sivuu 2:ca645c585a03 187 else {
sivuu 2:ca645c585a03 188 richting_motor1 = 1;
sivuu 2:ca645c585a03 189 }
sivuu 3:56f31420abac 190 if (voltage_motor2<0){
sivuu 3:56f31420abac 191 richting_motor2 = 0;
sivuu 3:56f31420abac 192 }
sivuu 3:56f31420abac 193 else {
sivuu 3:56f31420abac 194 richting_motor2 = 1;
sivuu 3:56f31420abac 195 }
sivuu 2:ca645c585a03 196 pwm_motor1 = fabs(voltage_motor1);
sivuu 3:56f31420abac 197 pwm_motor2 = fabs(voltage_motor2);
sivuu 2:ca645c585a03 198
sivuu 3:56f31420abac 199 pc.printf("%f", pwm_motor2.read());
sivuu 3:56f31420abac 200 pc.printf(" %f", d_ref2);
sivuu 3:56f31420abac 201 pc.printf(" %f \r\n", rev_counts_motor2_rad);
sivuu 3:56f31420abac 202
sivuu 0:57aa29917d4c 203 }
sivuu 0:57aa29917d4c 204 }