sibren vuurberg / Mbed 2 deprecated PID_controler

PID_controler voor het latere script

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@2:ca645c585a03, 2016-11-02 (annotated)

Committer:
sivuu
Date:
Wed Nov 02 16:58:55 2016 +0000
Revision:
2:ca645c585a03
Parent:
1:faa90344cdd8
Child:
3:56f31420abac
PID control, works! With limits both ways, one motor

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 0:57aa29917d4c 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 2:ca645c585a03 22 //HIDScope Scope(1);
sivuu 0:57aa29917d4c 23
sivuu 0:57aa29917d4c 24 //constanten voor de encoder
sivuu 0:57aa29917d4c 25 const int CW = 2.5; //definitie rechtsom 'lage waarde'
sivuu 0:57aa29917d4c 26 const int CCW =-1; //definitie linksom 'hoge waarde'
sivuu 0:57aa29917d4c 27 const float gearboxratio=131.25; // gearboxratio van encoder naar motor
sivuu 0:57aa29917d4c 28 const float rev_rond=64.0; // aantal revoluties per omgang van de encoder
sivuu 0:57aa29917d4c 29
sivuu 0:57aa29917d4c 30 volatile double current_position_motor1 = 0; // current position is 0 op het begin
sivuu 0:57aa29917d4c 31 volatile double previous_position_motor1 = 0; // previous position is 0 op het begin
sivuu 0:57aa29917d4c 32 volatile double current_position_motor2 = 0;
sivuu 0:57aa29917d4c 33 volatile double previous_position_motor2 = 0;
sivuu 0:57aa29917d4c 34 volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
sivuu 0:57aa29917d4c 35 volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 0:57aa29917d4c 36 volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 2:ca645c585a03 37 double ticker_TS=0.001; // zorgt voor een tijdstap van de ticker van 0.1 seconde
sivuu 0:57aa29917d4c 38 volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is
sivuu 0:57aa29917d4c 39 Ticker t; // maakt ticker t aan
sivuu 0:57aa29917d4c 40 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 41 volatile double value2_resetbutton = 0;
sivuu 0:57aa29917d4c 42
sivuu 0:57aa29917d4c 43
sivuu 0:57aa29917d4c 44 volatile float d_ref = 0;
sivuu 0:57aa29917d4c 45 volatile float d_ref_new;
sivuu 0:57aa29917d4c 46 const float w_ref = 3;
sivuu 2:ca645c585a03 47 const double Ts = 0.001;
sivuu 2:ca645c585a03 48 const double Kp = 0.3823;
sivuu 2:ca645c585a03 49 const double Ki = 0.127875;
sivuu 2:ca645c585a03 50 const double Kd = 0.2519;
sivuu 0:57aa29917d4c 51 const double N = 100;
sivuu 0:57aa29917d4c 52 Ticker tick;
sivuu 0:57aa29917d4c 53 Ticker controllerticker;
sivuu 0:57aa29917d4c 54 float rev_counts_motor1_rad;
sivuu 2:ca645c585a03 55 volatile float voltage_motor1=0.5;
sivuu 0:57aa29917d4c 56 volatile double error1;
sivuu 0:57aa29917d4c 57 volatile double ctrlOutput;
sivuu 2:ca645c585a03 58 volatile double d_ref_const_cw = 0;
sivuu 2:ca645c585a03 59 volatile double d_ref_const_ccw = 0;
sivuu 2:ca645c585a03 60
sivuu 0:57aa29917d4c 61
sivuu 0:57aa29917d4c 62 void reference(){
sivuu 2:ca645c585a03 63 if (button_cw == 0 ){
sivuu 2:ca645c585a03 64 d_ref = d_ref + w_ref * Ts;
sivuu 2:ca645c585a03 65 }
sivuu 2:ca645c585a03 66 if (d_ref > 21.36 ){
sivuu 2:ca645c585a03 67 d_ref = 21.36;
sivuu 2:ca645c585a03 68 //d_ref_const_cw = 1;
sivuu 2:ca645c585a03 69 }
sivuu 2:ca645c585a03 70 else{
sivuu 2:ca645c585a03 71 d_ref = d_ref;
sivuu 0:57aa29917d4c 72 }
sivuu 0:57aa29917d4c 73
sivuu 2:ca645c585a03 74 if (button_ccw == 0){
sivuu 2:ca645c585a03 75 d_ref = d_ref - w_ref * Ts;
sivuu 2:ca645c585a03 76 }
sivuu 2:ca645c585a03 77 if (d_ref < -21.36){
sivuu 2:ca645c585a03 78 d_ref = -21.36;
sivuu 2:ca645c585a03 79
sivuu 2:ca645c585a03 80 }
sivuu 2:ca645c585a03 81 else{
sivuu 2:ca645c585a03 82 d_ref = d_ref;
sivuu 2:ca645c585a03 83 }
sivuu 2:ca645c585a03 84
sivuu 2:ca645c585a03 85 }
sivuu 2:ca645c585a03 86
sivuu 0:57aa29917d4c 87 void m1_controller(){
sivuu 0:57aa29917d4c 88 error1 = d_ref-rev_counts_motor1_rad;
sivuu 0:57aa29917d4c 89 ctrlOutput = PID_controller.step(error1);
sivuu 2:ca645c585a03 90 }
sivuu 1:faa90344cdd8 91
sivuu 2:ca645c585a03 92 void tickerfunctie() // maakt een ticker functie aan
sivuu 2:ca645c585a03 93 {
sivuu 2:ca645c585a03 94 tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is
sivuu 2:ca645c585a03 95 }
sivuu 0:57aa29917d4c 96
sivuu 0:57aa29917d4c 97
sivuu 0:57aa29917d4c 98 int main()
sivuu 0:57aa29917d4c 99 {
sivuu 0:57aa29917d4c 100 pc.baud(115200);
sivuu 0:57aa29917d4c 101 QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
sivuu 2:ca645c585a03 102 int counts_encoder1; //variabele counts aanmaken
sivuu 0:57aa29917d4c 103 float rev_counts_motor1;
sivuu 2:ca645c585a03 104 t.attach(&tickerfunctie,ticker_TS);
sivuu 2:ca645c585a03 105 tick.attach(&reference,Ts);
sivuu 2:ca645c585a03 106 controllerticker.attach(&m1_controller,Ts);
sivuu 2:ca645c585a03 107 PID_controller.PIDF(Kp,Ki,Kd,N,Ts);
sivuu 0:57aa29917d4c 108
sivuu 0:57aa29917d4c 109 while (true) {
sivuu 2:ca645c585a03 110 // if (button_cw == 0){
sivuu 2:ca645c585a03 111 //if (tickerflag ==1){
sivuu 0:57aa29917d4c 112
sivuu 2:ca645c585a03 113 // richting_motor1 = 1;
sivuu 2:ca645c585a03 114 // pwm_motor1 = voltage_motor1;
sivuu 2:ca645c585a03 115 //pc.printf("pwm_motor is %f\r\n", pwm_motor1);
sivuu 2:ca645c585a03 116
sivuu 2:ca645c585a03 117 counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden
sivuu 2:ca645c585a03 118 rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond);
sivuu 2:ca645c585a03 119 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718;
sivuu 2:ca645c585a03 120 voltage_motor1=ctrlOutput;
sivuu 2:ca645c585a03 121 if (voltage_motor1<0){
sivuu 2:ca645c585a03 122 richting_motor1 = 0;
sivuu 2:ca645c585a03 123 }
sivuu 2:ca645c585a03 124 else {
sivuu 2:ca645c585a03 125 richting_motor1 = 1;
sivuu 2:ca645c585a03 126 }
sivuu 2:ca645c585a03 127 pwm_motor1 = fabs(voltage_motor1);
sivuu 2:ca645c585a03 128
sivuu 2:ca645c585a03 129 pc.printf("%f", pwm_motor1.read());
sivuu 2:ca645c585a03 130 pc.printf(" %f", d_ref);
sivuu 2:ca645c585a03 131 pc.printf(" %f \r\n", rev_counts_motor1_rad);
sivuu 2:ca645c585a03 132 //Scope.set(0, rev_counts_motor1_rad);
sivuu 2:ca645c585a03 133 //Scope.send();
sivuu 2:ca645c585a03 134 //tickerflag = 0;
sivuu 2:ca645c585a03 135 // d_ref_const_ccw = 0;
sivuu 2:ca645c585a03 136 //}
sivuu 2:ca645c585a03 137 // }
sivuu 2:ca645c585a03 138 // else if (button_ccw == 0){
sivuu 0:57aa29917d4c 139
sivuu 2:ca645c585a03 140 /* richting_motor1 = 0;
sivuu 2:ca645c585a03 141 pwm_motor1 = voltage_motor1;
sivuu 2:ca645c585a03 142 //pc.printf("pwm_motor is %f\r\n", pwm_motor1);
sivuu 2:ca645c585a03 143
sivuu 2:ca645c585a03 144 counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden
sivuu 2:ca645c585a03 145 rev_counts_motor1=(float)counts_encoder1/(gearboxratio*rev_rond);
sivuu 2:ca645c585a03 146 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718;
sivuu 2:ca645c585a03 147 voltage_motor1=ctrlOutput;
sivuu 2:ca645c585a03 148 pc.printf("%f", pwm_motor1);
sivuu 2:ca645c585a03 149 pc.printf(" %f", d_ref);
sivuu 2:ca645c585a03 150 pc.printf(" %f \r\n", rev_counts_motor1_rad);
sivuu 2:ca645c585a03 151 //Scope.set(0, rev_counts_motor1_rad);
sivuu 2:ca645c585a03 152 //Scope.send();
sivuu 2:ca645c585a03 153 //tickerflag = 0;
sivuu 2:ca645c585a03 154 d_ref_const_cw = 0; */
sivuu 2:ca645c585a03 155 // }
sivuu 2:ca645c585a03 156
sivuu 2:ca645c585a03 157 // else{
sivuu 2:ca645c585a03 158 // pwm_motor1=0;
sivuu 2:ca645c585a03 159
sivuu 2:ca645c585a03 160 //}
sivuu 0:57aa29917d4c 161 }
sivuu 0:57aa29917d4c 162 }