sibren vuurberg / Mbed 2 deprecated PID_controler

PID_controler voor het latere script

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@1:faa90344cdd8, 2016-11-02 (annotated)

Committer:
sivuu
Date:
Wed Nov 02 10:50:35 2016 +0000
Revision:
1:faa90344cdd8
Parent:
0:57aa29917d4c
Child:
2:ca645c585a03
PID_controler

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 1:faa90344cdd8 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 0:57aa29917d4c 37 double ticker_TS=0.025; // 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 0:57aa29917d4c 47 const double Fs = 0.001;
sivuu 1:faa90344cdd8 48 const double Kp = 0.5;
sivuu 0:57aa29917d4c 49 const double Ki = 5;
sivuu 0:57aa29917d4c 50 const double Kd = 5;
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 0:57aa29917d4c 55 volatile float voltage_motor1=0.18;
sivuu 0:57aa29917d4c 56 volatile double error1;
sivuu 0:57aa29917d4c 57 volatile double ctrlOutput;
sivuu 1:faa90344cdd8 58 volatile double sintel =0;
sivuu 0:57aa29917d4c 59
sivuu 0:57aa29917d4c 60 void reference(){
sivuu 0:57aa29917d4c 61 d_ref = d_ref + w_ref * Fs;
sivuu 0:57aa29917d4c 62 }
sivuu 0:57aa29917d4c 63
sivuu 0:57aa29917d4c 64 void m1_controller(){
sivuu 0:57aa29917d4c 65 error1 = d_ref-rev_counts_motor1_rad;
sivuu 0:57aa29917d4c 66 ctrlOutput = PID_controller.step(error1);
sivuu 1:faa90344cdd8 67 }
sivuu 1:faa90344cdd8 68 void sin_teller(){
sivuu 1:faa90344cdd8 69 sintel=sintel+0.01;
sivuu 1:faa90344cdd8 70 }
sivuu 0:57aa29917d4c 71 // if (ctrlOutput > 1){
sivuu 0:57aa29917d4c 72 // ctrlOutput =1;
sivuu 0:57aa29917d4c 73 // }
sivuu 0:57aa29917d4c 74 // if (ctrlOutput < 0.01){
sivuu 0:57aa29917d4c 75 // ctrlOutput = 0.01;
sivuu 0:57aa29917d4c 76 // }
sivuu 0:57aa29917d4c 77 // else{
sivuu 0:57aa29917d4c 78 // ctrlOutput = ctrlOutput;
sivuu 0:57aa29917d4c 79 // }
sivuu 1:faa90344cdd8 80
sivuu 0:57aa29917d4c 81
sivuu 0:57aa29917d4c 82
sivuu 0:57aa29917d4c 83
sivuu 0:57aa29917d4c 84 int main()
sivuu 0:57aa29917d4c 85 {
sivuu 0:57aa29917d4c 86 pc.baud(115200);
sivuu 0:57aa29917d4c 87 QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
sivuu 0:57aa29917d4c 88 float counts_encoder1; //variabele counts aanmaken
sivuu 0:57aa29917d4c 89 float rev_counts_motor1;
sivuu 0:57aa29917d4c 90
sivuu 0:57aa29917d4c 91
sivuu 0:57aa29917d4c 92 while (true) {
sivuu 0:57aa29917d4c 93 if (button_cw == 0){
sivuu 1:faa90344cdd8 94 t.attach(&sin_teller,1);
sivuu 0:57aa29917d4c 95 richting_motor1 = 1;
sivuu 1:faa90344cdd8 96 pwm_motor1 = sin(sintel*10*3.141592653);
sivuu 1:faa90344cdd8 97 pc.printf("pwm_motor is %f\r\n", pwm_motor1);
sivuu 1:faa90344cdd8 98 pc.printf("sintel is %f\r\n", sintel);
sivuu 0:57aa29917d4c 99 counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden
sivuu 0:57aa29917d4c 100 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
sivuu 0:57aa29917d4c 101 rev_counts_motor1_rad=rev_counts_motor1*6.28318530718;
sivuu 1:faa90344cdd8 102 //tick.attach(&reference,Fs);
sivuu 1:faa90344cdd8 103 //PID_controller.PIDF(Kp,Ki,Kd,N,Fs);
sivuu 1:faa90344cdd8 104 //controllerticker.attach(&m1_controller,Fs);
sivuu 1:faa90344cdd8 105 //voltage_motor1=ctrlOutput;
sivuu 1:faa90344cdd8 106 //pc.printf("%f", voltage_motor1);
sivuu 1:faa90344cdd8 107 //pc.printf(" %f", d_ref);
sivuu 1:faa90344cdd8 108 //pc.printf(" %f \r\n", rev_counts_motor1_rad);
sivuu 1:faa90344cdd8 109 Scope.set(0, rev_counts_motor1_rad);
sivuu 1:faa90344cdd8 110 Scope.send();
sivuu 0:57aa29917d4c 111 }
sivuu 0:57aa29917d4c 112
sivuu 0:57aa29917d4c 113 else{
sivuu 0:57aa29917d4c 114 pwm_motor1=0;
sivuu 0:57aa29917d4c 115
sivuu 0:57aa29917d4c 116 }
sivuu 0:57aa29917d4c 117 }
sivuu 0:57aa29917d4c 118 }