Bruno Tomić / Mbed 2 deprecated Zavrsni_rad_NXP_cup

Završni rad NXP Cup

Dependencies:   HBridgeDCMotor mbed FastPWM AutomationElements

main.cpp@6:e1e317fe8f7f, 2016-07-14 (annotated)

Committer:
btomic
Date:
Thu Jul 14 10:49:40 2016 +0000
Revision:
6:e1e317fe8f7f
Parent:
5:d3bdc0672891
Child:
7:b881681f7ff6
Testna verzija 7

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 6:e1e317fe8f7f 43 float u_R_a = 0, u_R_b = 0, x=0, Ubat = 0, R_au = 0.9, L_au = 218e-6, K = 2.1856e-3, u_i_a2 = 0, w = 0, N = 7.46, n = 0, delay = 0.13;
btomic 6:e1e317fe8f7f 44 double u_i_a = 0, u_i_b = 0, e = 0;
btomic 5:d3bdc0672891 45 double T_d = 1e-3;
btomic 5:d3bdc0672891 46
btomic 5:d3bdc0672891 47 PI PI_a (0.04, 0.001, T_d);
btomic 5:d3bdc0672891 48 PI PI_b (0.04, 0.001, T_d);
btomic 2:25b8be806708 49
btomic 6:e1e317fe8f7f 50 float polje[]={0.3,0.4};
btomic 6:e1e317fe8f7f 51 int index=0;
btomic 6:e1e317fe8f7f 52
btomic 6:e1e317fe8f7f 53 void changeRef(){
btomic 6:e1e317fe8f7f 54 index = ++index%2;
btomic 6:e1e317fe8f7f 55 x = polje[index];
btomic 6:e1e317fe8f7f 56 }
btomic 5:d3bdc0672891 57 void tick(){
btomic 6:e1e317fe8f7f 58 if ((u_R_a - delay) <= 0) u_i_a = 0;
btomic 6:e1e317fe8f7f 59 else u_i_a = 375.0/220.0*(3.3 * a_feedback)/(u_R_a - delay);
btomic 6:e1e317fe8f7f 60 if ((u_R_b - delay) <= 0) u_i_b = 0;
btomic 6:e1e317fe8f7f 61 else u_i_b = 375.0/220.0*(3.3 * b_feedback)/(u_R_b - delay);
btomic 5:d3bdc0672891 62 x = 3 * pot2;
btomic 6:e1e317fe8f7f 63 //x = 0.4;
btomic 5:d3bdc0672891 64 PI_a.in(x - u_i_a);
btomic 5:d3bdc0672891 65 PI_b.in(x - u_i_b);
btomic 5:d3bdc0672891 66 u_R_a = PI_a.out();
btomic 5:d3bdc0672891 67 u_R_b = PI_b.out();
btomic 6:e1e317fe8f7f 68
btomic 6:e1e317fe8f7f 69 motorA.pulsewidth(100e-6*u_R_a);
btomic 6:e1e317fe8f7f 70 //motorB.pulsewidth(100e-6*u_R_b);
btomic 5:d3bdc0672891 71
btomic 6:e1e317fe8f7f 72 Ubat = naponBaterije * 3.3 * (57.0/10.0);
btomic 6:e1e317fe8f7f 73 if(u_i_a < 0.1 + u_i_a2 && u_i_a > u_i_a2 - 0.1){
btomic 6:e1e317fe8f7f 74 e = Ubat*(u_R_a - delay) - R_au*u_i_a - L_au * ((u_i_a - u_i_a2)/T_d);
btomic 6:e1e317fe8f7f 75 w = e / K;
btomic 6:e1e317fe8f7f 76 n = w * (30/3.14159) / N;
btomic 6:e1e317fe8f7f 77 u_i_a2 = u_i_a;
btomic 6:e1e317fe8f7f 78
btomic 6:e1e317fe8f7f 79 test_w.pulsewidth_us(n);
btomic 6:e1e317fe8f7f 80 }
btomic 5:d3bdc0672891 81 }
btomic 6:e1e317fe8f7f 82 /**
btomic 6:e1e317fe8f7f 83 void camSI(){
btomic 6:e1e317fe8f7f 84 cameraCLK.pulsewidth(1.0/100e3 * 0.5);
btomic 6:e1e317fe8f7f 85 if (cameraCLK = 1) SI = 1
btomic 6:e1e317fe8f7f 86
btomic 6:e1e317fe8f7f 87 }
btomic 6:e1e317fe8f7f 88 **/
btomic 3:fc17c2ad6b81 89 void steerServo(float angleDegrees){
btomic 4:d3c4924179f0 90 float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0;
btomic 5:d3bdc0672891 91 if (angleDegrees < -29 ) angleDegrees = -29;
btomic 5:d3bdc0672891 92 if (angleDegrees > 30 ) angleDegrees = 30;
btomic 3:fc17c2ad6b81 93 pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng);
btomic 3:fc17c2ad6b81 94 servo.pulsewidth(pulse);
btomic 1:50fc6689148e 95 }
btomic 0:1b75c7a92a5e 96
btomic 0:1b75c7a92a5e 97 int main() {
btomic 6:e1e317fe8f7f 98 float PWmin=-90, PWmax=90, angleDegrees = 0;
btomic 6:e1e317fe8f7f 99
btomic 6:e1e317fe8f7f 100 // cameraCLK.period(1.0/100e3); // frekvencija 100 kHz
btomic 4:d3c4924179f0 101
btomic 6:e1e317fe8f7f 102 //servo.period(1.0/50);
btomic 6:e1e317fe8f7f 103 //steerServo(angleDegrees);
btomic 6:e1e317fe8f7f 104
btomic 6:e1e317fe8f7f 105 PI_a.setOutputLimits (0, 1);
btomic 6:e1e317fe8f7f 106 PI_b.setOutputLimits (0, 1);
btomic 6:e1e317fe8f7f 107
btomic 6:e1e317fe8f7f 108 motorA.period_us(100);
btomic 6:e1e317fe8f7f 109 motorB.period_us(100);
btomic 6:e1e317fe8f7f 110 test_w.period_us(10000);
btomic 5:d3bdc0672891 111
btomic 6:e1e317fe8f7f 112 wait(3);
btomic 5:d3bdc0672891 113 enable = 1;
btomic 6:e1e317fe8f7f 114 motorB_grana2 = 0;
btomic 6:e1e317fe8f7f 115 motorA_grana2 = 0;
btomic 5:d3bdc0672891 116 ticker.attach(&tick, T_d);
btomic 6:e1e317fe8f7f 117 //tikref.attach(&changeRef,1);
btomic 6:e1e317fe8f7f 118 // cameraSI.attach(&camSI, 0.00258);
btomic 6:e1e317fe8f7f 119 float analogprint, analogprintb;
btomic 6:e1e317fe8f7f 120 while(1){
btomic 6:e1e317fe8f7f 121 if(sw1 == 1)enable = 0;
btomic 6:e1e317fe8f7f 122 if(fault == 1) {
btomic 6:e1e317fe8f7f 123 d1=1;
btomic 6:e1e317fe8f7f 124 d3=0;
btomic 6:e1e317fe8f7f 125 }
btomic 6:e1e317fe8f7f 126 else {
btomic 6:e1e317fe8f7f 127 d3=1;
btomic 6:e1e317fe8f7f 128 d1=0;
btomic 6:e1e317fe8f7f 129 }
btomic 6:e1e317fe8f7f 130 /**
btomic 6:e1e317fe8f7f 131 angleDegrees = PWmin + (PWmax - PWmin) * pot1;
btomic 6:e1e317fe8f7f 132 steerServo(angleDegrees);
btomic 6:e1e317fe8f7f 133 **/
btomic 4:d3c4924179f0 134
btomic 6:e1e317fe8f7f 135
btomic 5:d3bdc0672891 136
btomic 6:e1e317fe8f7f 137 wait(1);
btomic 4:d3c4924179f0 138
btomic 5:d3bdc0672891 139
btomic 1:50fc6689148e 140
btomic 6:e1e317fe8f7f 141 analogprint = a_feedback;
btomic 6:e1e317fe8f7f 142 analogprintb = b_feedback;
btomic 6:e1e317fe8f7f 143 pc.printf("a_feedback: %f b_feedback: %f \n\r",analogprint, analogprintb);
btomic 4:d3c4924179f0 144
btomic 0:1b75c7a92a5e 145 }
btomic 0:1b75c7a92a5e 146 }