Završni rad NXP Cup
Dependencies: HBridgeDCMotor mbed FastPWM AutomationElements
main.cpp
- Committer:
- btomic
- Date:
- 2016-07-14
- Revision:
- 6:e1e317fe8f7f
- Parent:
- 5:d3bdc0672891
- Child:
- 7:b881681f7ff6
File content as of revision 6:e1e317fe8f7f:
#include "mbed.h" #include "FastPWM.h" #include "AutomationElements.h" DigitalIn fault(PTE20); DigitalIn sw1(PTC13); DigitalIn sw2(PTC17); AnalogIn pte23(PTE23); //bugirani analogini ulaz, zamjena PTE30 AnalogIn a_feedback(PTE30); AnalogIn b_feedback(PTE22); AnalogIn pot1(PTB3); AnalogIn pot2(PTB2); AnalogIn camera(PTD5); AnalogIn naponBaterije(PTE29); Serial pc(PTA2, PTA1); DigitalOut enable(PTE21); DigitalOut d1(PTB8); DigitalOut d2(PTB9); DigitalOut d3(PTB10); DigitalOut d4(PTB11); //DigitalOut red(PTB18); //DigitalOut green(PTB19); //DigitalOut blue(PTD1); /** DigiralOut SI(PTD7); // camera SI PwmOut cameraCLK(PTE1); **/ PwmOut servo(PTB0); FastPWM motorA(PTC3); DigitalOut motorA_grana2(PTC4); FastPWM motorB(PTC1); DigitalOut motorB_grana2(PTC2); FastPWM test_w(PTA12); Ticker ticker; Ticker tikref; //Ticker cameraSI; 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; double u_i_a = 0, u_i_b = 0, e = 0; double T_d = 1e-3; PI PI_a (0.04, 0.001, T_d); PI PI_b (0.04, 0.001, T_d); float polje[]={0.3,0.4}; int index=0; void changeRef(){ index = ++index%2; x = polje[index]; } void tick(){ if ((u_R_a - delay) <= 0) u_i_a = 0; else u_i_a = 375.0/220.0*(3.3 * a_feedback)/(u_R_a - delay); if ((u_R_b - delay) <= 0) u_i_b = 0; else u_i_b = 375.0/220.0*(3.3 * b_feedback)/(u_R_b - delay); x = 3 * pot2; //x = 0.4; PI_a.in(x - u_i_a); PI_b.in(x - u_i_b); u_R_a = PI_a.out(); u_R_b = PI_b.out(); motorA.pulsewidth(100e-6*u_R_a); //motorB.pulsewidth(100e-6*u_R_b); Ubat = naponBaterije * 3.3 * (57.0/10.0); if(u_i_a < 0.1 + u_i_a2 && u_i_a > u_i_a2 - 0.1){ e = Ubat*(u_R_a - delay) - R_au*u_i_a - L_au * ((u_i_a - u_i_a2)/T_d); w = e / K; n = w * (30/3.14159) / N; u_i_a2 = u_i_a; test_w.pulsewidth_us(n); } } /** void camSI(){ cameraCLK.pulsewidth(1.0/100e3 * 0.5); if (cameraCLK = 1) SI = 1 } **/ void steerServo(float angleDegrees){ float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0; if (angleDegrees < -29 ) angleDegrees = -29; if (angleDegrees > 30 ) angleDegrees = 30; pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng); servo.pulsewidth(pulse); } int main() { float PWmin=-90, PWmax=90, angleDegrees = 0; // cameraCLK.period(1.0/100e3); // frekvencija 100 kHz //servo.period(1.0/50); //steerServo(angleDegrees); PI_a.setOutputLimits (0, 1); PI_b.setOutputLimits (0, 1); motorA.period_us(100); motorB.period_us(100); test_w.period_us(10000); wait(3); enable = 1; motorB_grana2 = 0; motorA_grana2 = 0; ticker.attach(&tick, T_d); //tikref.attach(&changeRef,1); // cameraSI.attach(&camSI, 0.00258); float analogprint, analogprintb; while(1){ if(sw1 == 1)enable = 0; if(fault == 1) { d1=1; d3=0; } else { d3=1; d1=0; } /** angleDegrees = PWmin + (PWmax - PWmin) * pot1; steerServo(angleDegrees); **/ wait(1); analogprint = a_feedback; analogprintb = b_feedback; pc.printf("a_feedback: %f b_feedback: %f \n\r",analogprint, analogprintb); } }