Bruno Tomić / Mbed 2 deprecated Zavrsni_rad_NXP_cup

Završni rad NXP Cup

Dependencies:   HBridgeDCMotor mbed FastPWM AutomationElements

main.cpp@7:b881681f7ff6, 2016-07-22 (annotated)

Committer:
btomic
Date:
Fri Jul 22 09:35:59 2016 +0000
Revision:
7:b881681f7ff6
Parent:
6:e1e317fe8f7f 
Kod prije pretvaranja u klasu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
btomic 0:1b75c7a92a5e 1 #include "mbed.h"
btomic 4:d3c4924179f0 2 #include "FastPWM.h"
btomic 5:d3bdc0672891 3 #include "AutomationElements.h"
btomic 0:1b75c7a92a5e 4
btomic 0:1b75c7a92a5e 5 DigitalIn fault(PTE20);
btomic 4:d3c4924179f0 6 DigitalIn sw1(PTC13);
btomic 4:d3c4924179f0 7 DigitalIn sw2(PTC17);
btomic 6:e1e317fe8f7f 8 AnalogIn pte23(PTE23); //bugirani analogini ulaz, zamjena PTE30
btomic 6:e1e317fe8f7f 9 AnalogIn a_feedback(PTE30);
btomic 1:50fc6689148e 10 AnalogIn b_feedback(PTE22);
btomic 4:d3c4924179f0 11 AnalogIn pot1(PTB3);
btomic 4:d3c4924179f0 12 AnalogIn pot2(PTB2);
btomic 6:e1e317fe8f7f 13 AnalogIn camera(PTD5);
btomic 6:e1e317fe8f7f 14 AnalogIn naponBaterije(PTE29);
btomic 4:d3c4924179f0 15
btomic 4:d3c4924179f0 16 Serial pc(PTA2, PTA1);
btomic 4:d3c4924179f0 17
btomic 2:25b8be806708 18 DigitalOut enable(PTE21);
btomic 2:25b8be806708 19 DigitalOut d1(PTB8);
btomic 1:50fc6689148e 20 DigitalOut d2(PTB9);
btomic 1:50fc6689148e 21 DigitalOut d3(PTB10);
btomic 1:50fc6689148e 22 DigitalOut d4(PTB11);
btomic 1:50fc6689148e 23 //DigitalOut red(PTB18);
btomic 1:50fc6689148e 24 //DigitalOut green(PTB19);
btomic 1:50fc6689148e 25 //DigitalOut blue(PTD1);
btomic 6:e1e317fe8f7f 26 /**
btomic 6:e1e317fe8f7f 27 DigiralOut SI(PTD7); // camera SI
btomic 1:50fc6689148e 28
btomic 6:e1e317fe8f7f 29 PwmOut cameraCLK(PTE1);
btomic 6:e1e317fe8f7f 30 **/
btomic 0:1b75c7a92a5e 31 PwmOut servo(PTB0);
btomic 2:25b8be806708 32
btomic 6:e1e317fe8f7f 33 FastPWM motorA(PTC3);
btomic 6:e1e317fe8f7f 34 DigitalOut motorA_grana2(PTC4);
btomic 6:e1e317fe8f7f 35 FastPWM motorB(PTC1);
btomic 6:e1e317fe8f7f 36 DigitalOut motorB_grana2(PTC2);
btomic 6:e1e317fe8f7f 37 FastPWM test_w(PTA12);
btomic 5:d3bdc0672891 38
btomic 5:d3bdc0672891 39 Ticker ticker;
btomic 6:e1e317fe8f7f 40 Ticker tikref;
btomic 6:e1e317fe8f7f 41 //Ticker cameraSI;
btomic 6:e1e317fe8f7f 42
btomic 7:b881681f7ff6 43 float u_Ri_a = 0, u_Ri_b = 0, n_ref=0, Ubat = 0, R_au = 1.067, L_au = 218e-6, K = 2.54e-3, pot2_skalirani = 0;
btomic 7:b881681f7ff6 44 float u_i_a2 = 0, u_i_b2 = 0, w_a = 0, w_b = 0, N = 7.46, n_a = 0, n_b = 0, delay = 0.13, u_Rw_a = 0, u_Rw_b =0;
btomic 7:b881681f7ff6 45 double u_i_a = 0, u_i_b = 0, e_a = 0, e_b = 0;
btomic 5:d3bdc0672891 46 double T_d = 1e-3;
btomic 5:d3bdc0672891 47
btomic 5:d3bdc0672891 48 PI PI_a (0.04, 0.001, T_d);
btomic 5:d3bdc0672891 49 PI PI_b (0.04, 0.001, T_d);
btomic 2:25b8be806708 50
btomic 7:b881681f7ff6 51 PI RegulatorBrzine_a( 0.01, 0.01, T_d);
btomic 7:b881681f7ff6 52 PI RegulatorBrzine_b( 0.01, 0.01, T_d);
btomic 6:e1e317fe8f7f 53
btomic 7:b881681f7ff6 54 Timer timer;
btomic 7:b881681f7ff6 55
btomic 7:b881681f7ff6 56
btomic 5:d3bdc0672891 57 void tick(){
btomic 7:b881681f7ff6 58 timer.start();
btomic 7:b881681f7ff6 59 Ubat = naponBaterije * 3.3 * (57.0/10.0);
btomic 7:b881681f7ff6 60 timer.stop();
btomic 7:b881681f7ff6 61 test_w.pulsewidth_us(timer.read_us());
btomic 7:b881681f7ff6 62 timer.reset();
btomic 7:b881681f7ff6 63 if ((u_Ri_a - delay) <= 0) u_i_a = 0;
btomic 7:b881681f7ff6 64 else u_i_a = 375.0/220.0*(3.3 * a_feedback)/(u_Ri_a - delay);
btomic 7:b881681f7ff6 65 if ((u_Ri_b - delay) <= 0) u_i_b = 0;
btomic 7:b881681f7ff6 66 else u_i_b = 375.0/220.0*(3.3 * b_feedback)/(u_Ri_b - delay);
btomic 7:b881681f7ff6 67 //pot2_skalirani = 100 + 1900* pot2;
btomic 7:b881681f7ff6 68 //x = pot2_skalirani;
btomic 7:b881681f7ff6 69 //x = 3000 * pot2;
btomic 7:b881681f7ff6 70 n_ref = 1000;
btomic 7:b881681f7ff6 71
btomic 7:b881681f7ff6 72
btomic 7:b881681f7ff6 73 e_a = Ubat*(u_Ri_a - delay) - R_au*u_i_a - L_au * ((u_i_a - u_i_a2)/T_d);
btomic 7:b881681f7ff6 74 w_a = e_a / K;
btomic 7:b881681f7ff6 75 n_a = w_a * (30/3.14159) / N;
btomic 7:b881681f7ff6 76 u_i_a2 = u_i_a;
btomic 7:b881681f7ff6 77 e_b = Ubat*(u_Ri_b - delay) - R_au*u_i_b - L_au * ((u_i_b - u_i_b2)/T_d);
btomic 7:b881681f7ff6 78 w_b = e_b / K;
btomic 7:b881681f7ff6 79 n_b = w_b * (30/3.14159) / N;
btomic 7:b881681f7ff6 80 u_i_b2 = u_i_b;
btomic 7:b881681f7ff6 81
btomic 7:b881681f7ff6 82 RegulatorBrzine_a.in(n_ref - n_a);
btomic 7:b881681f7ff6 83 RegulatorBrzine_b.in(n_ref - n_b);
btomic 7:b881681f7ff6 84
btomic 7:b881681f7ff6 85 u_Rw_a = RegulatorBrzine_a.out();
btomic 7:b881681f7ff6 86 u_Rw_b = RegulatorBrzine_b.out();
btomic 7:b881681f7ff6 87
btomic 7:b881681f7ff6 88 PI_a.in(u_Rw_a - u_i_a);
btomic 7:b881681f7ff6 89 PI_b.in(u_Rw_b - u_i_b);
btomic 7:b881681f7ff6 90 u_Ri_a = PI_a.out();
btomic 7:b881681f7ff6 91 u_Ri_b = PI_b.out();
btomic 7:b881681f7ff6 92
btomic 7:b881681f7ff6 93 motorA.pulsewidth(100e-6*u_Ri_a);
btomic 7:b881681f7ff6 94 motorB.pulsewidth(100e-6*u_Ri_b);
btomic 6:e1e317fe8f7f 95
btomic 7:b881681f7ff6 96
btomic 5:d3bdc0672891 97 }
btomic 6:e1e317fe8f7f 98 /**
btomic 6:e1e317fe8f7f 99 void camSI(){
btomic 6:e1e317fe8f7f 100 cameraCLK.pulsewidth(1.0/100e3 * 0.5);
btomic 6:e1e317fe8f7f 101 if (cameraCLK = 1) SI = 1
btomic 6:e1e317fe8f7f 102
btomic 6:e1e317fe8f7f 103 }
btomic 6:e1e317fe8f7f 104 **/
btomic 3:fc17c2ad6b81 105 void steerServo(float angleDegrees){
btomic 4:d3c4924179f0 106 float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0;
btomic 5:d3bdc0672891 107 if (angleDegrees < -29 ) angleDegrees = -29;
btomic 5:d3bdc0672891 108 if (angleDegrees > 30 ) angleDegrees = 30;
btomic 3:fc17c2ad6b81 109 pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng);
btomic 3:fc17c2ad6b81 110 servo.pulsewidth(pulse);
btomic 1:50fc6689148e 111 }
btomic 0:1b75c7a92a5e 112
btomic 0:1b75c7a92a5e 113 int main() {
btomic 6:e1e317fe8f7f 114 float PWmin=-90, PWmax=90, angleDegrees = 0;
btomic 6:e1e317fe8f7f 115
btomic 6:e1e317fe8f7f 116 // cameraCLK.period(1.0/100e3); // frekvencija 100 kHz
btomic 4:d3c4924179f0 117
btomic 6:e1e317fe8f7f 118 //servo.period(1.0/50);
btomic 6:e1e317fe8f7f 119 //steerServo(angleDegrees);
btomic 6:e1e317fe8f7f 120
btomic 6:e1e317fe8f7f 121 PI_a.setOutputLimits (0, 1);
btomic 6:e1e317fe8f7f 122 PI_b.setOutputLimits (0, 1);
btomic 7:b881681f7ff6 123
btomic 7:b881681f7ff6 124 RegulatorBrzine_a.setOutputLimits (0,3);
btomic 7:b881681f7ff6 125 RegulatorBrzine_b.setOutputLimits(0,3);
btomic 7:b881681f7ff6 126
btomic 6:e1e317fe8f7f 127 motorA.period_us(100);
btomic 6:e1e317fe8f7f 128 motorB.period_us(100);
btomic 6:e1e317fe8f7f 129 test_w.period_us(10000);
btomic 5:d3bdc0672891 130
btomic 6:e1e317fe8f7f 131 wait(3);
btomic 5:d3bdc0672891 132 enable = 1;
btomic 6:e1e317fe8f7f 133 motorB_grana2 = 0;
btomic 6:e1e317fe8f7f 134 motorA_grana2 = 0;
btomic 5:d3bdc0672891 135 ticker.attach(&tick, T_d);
btomic 6:e1e317fe8f7f 136 //tikref.attach(&changeRef,1);
btomic 6:e1e317fe8f7f 137 // cameraSI.attach(&camSI, 0.00258);
btomic 6:e1e317fe8f7f 138 float analogprint, analogprintb;
btomic 6:e1e317fe8f7f 139 while(1){
btomic 7:b881681f7ff6 140 if(sw1 == 1) enable = 0;
btomic 7:b881681f7ff6 141 if(sw2 == 1) enable = 1;
btomic 6:e1e317fe8f7f 142 if(fault == 1) {
btomic 6:e1e317fe8f7f 143 d1=1;
btomic 6:e1e317fe8f7f 144 d3=0;
btomic 6:e1e317fe8f7f 145 }
btomic 6:e1e317fe8f7f 146 else {
btomic 6:e1e317fe8f7f 147 d3=1;
btomic 6:e1e317fe8f7f 148 d1=0;
btomic 6:e1e317fe8f7f 149 }
btomic 6:e1e317fe8f7f 150 /**
btomic 6:e1e317fe8f7f 151 angleDegrees = PWmin + (PWmax - PWmin) * pot1;
btomic 6:e1e317fe8f7f 152 steerServo(angleDegrees);
btomic 6:e1e317fe8f7f 153 **/
btomic 4:d3c4924179f0 154
btomic 6:e1e317fe8f7f 155
btomic 5:d3bdc0672891 156
btomic 6:e1e317fe8f7f 157 wait(1);
btomic 4:d3c4924179f0 158
btomic 5:d3bdc0672891 159
btomic 1:50fc6689148e 160
btomic 6:e1e317fe8f7f 161 analogprint = a_feedback;
btomic 6:e1e317fe8f7f 162 analogprintb = b_feedback;
btomic 6:e1e317fe8f7f 163 pc.printf("a_feedback: %f b_feedback: %f \n\r",analogprint, analogprintb);
btomic 4:d3c4924179f0 164
btomic 0:1b75c7a92a5e 165 }
btomic 0:1b75c7a92a5e 166 }