Bruno Tomić / Mbed 2 deprecated Zavrsni_rad_NXP_cup

Završni rad NXP Cup

Dependencies:   HBridgeDCMotor mbed FastPWM AutomationElements

main.cpp@5:d3bdc0672891, 2016-06-14 (annotated)

Committer:
btomic
Date:
Tue Jun 14 16:02:25 2016 +0000
Revision:
5:d3bdc0672891
Parent:
4:d3c4924179f0
Child:
6:e1e317fe8f7f
Testna verzija 6

Who changed what in which revision?

UserRevisionLine numberNew contents of line
btomic 0:1b75c7a92a5e 1 #include "mbed.h"
btomic 0:1b75c7a92a5e 2 #include "HBridgeDCMotor.h"
btomic 4:d3c4924179f0 3 #include "FastPWM.h"
btomic 5:d3bdc0672891 4 #include "AutomationElements.h"
btomic 0:1b75c7a92a5e 5
btomic 0:1b75c7a92a5e 6 DigitalIn fault(PTE20);
btomic 4:d3c4924179f0 7 DigitalIn sw1(PTC13);
btomic 4:d3c4924179f0 8 DigitalIn sw2(PTC17);
btomic 1:50fc6689148e 9 AnalogIn a_feedback(PTE23);
btomic 1:50fc6689148e 10 AnalogIn b_feedback(PTE22);
btomic 4:d3c4924179f0 11 AnalogIn pot1(PTB3);
btomic 4:d3c4924179f0 12 AnalogIn pot2(PTB2);
btomic 4:d3c4924179f0 13
btomic 4:d3c4924179f0 14 Serial pc(PTA2, PTA1);
btomic 4:d3c4924179f0 15
btomic 0:1b75c7a92a5e 16
btomic 2:25b8be806708 17 DigitalOut enable(PTE21);
btomic 2:25b8be806708 18 DigitalOut d1(PTB8);
btomic 1:50fc6689148e 19 DigitalOut d2(PTB9);
btomic 1:50fc6689148e 20 DigitalOut d3(PTB10);
btomic 1:50fc6689148e 21 DigitalOut d4(PTB11);
btomic 1:50fc6689148e 22 //DigitalOut red(PTB18);
btomic 1:50fc6689148e 23 //DigitalOut green(PTB19);
btomic 1:50fc6689148e 24 //DigitalOut blue(PTD1);
btomic 1:50fc6689148e 25
btomic 1:50fc6689148e 26 //HBridgeDCMotor motor_a(PTC3, PTC4);
btomic 1:50fc6689148e 27 //HBridgeDCMotor motor_b(PTC1, PTC2);
btomic 0:1b75c7a92a5e 28
btomic 0:1b75c7a92a5e 29 PwmOut servo(PTB0);
btomic 2:25b8be806708 30
btomic 4:d3c4924179f0 31 FastPWM motor1(PTC3);
btomic 5:d3bdc0672891 32 DigitalOut motor1_grana2(PTC4);
btomic 4:d3c4924179f0 33 FastPWM motor2(PTC1);
btomic 5:d3bdc0672891 34 DigitalOut motor2_grana2(PTC2);
btomic 5:d3bdc0672891 35
btomic 5:d3bdc0672891 36 Ticker ticker;
btomic 5:d3bdc0672891 37 float u_R_a = 0.3, u_R_b = 0.3, x=0;
btomic 5:d3bdc0672891 38 double u_i_a = 0, u_i_b = 0;
btomic 5:d3bdc0672891 39 double T_d = 1e-3;
btomic 5:d3bdc0672891 40
btomic 5:d3bdc0672891 41 PI PI_a (0.04, 0.001, T_d);
btomic 5:d3bdc0672891 42 PI PI_b (0.04, 0.001, T_d);
btomic 2:25b8be806708 43
btomic 5:d3bdc0672891 44 void tick(){
btomic 5:d3bdc0672891 45 u_i_a = 375.0/220.0*(3.3 * a_feedback)/u_R_a;
btomic 5:d3bdc0672891 46 u_i_b = 375.0/220.0*(3.3 * b_feedback)/u_R_b;
btomic 5:d3bdc0672891 47 x = 3 * pot2;
btomic 5:d3bdc0672891 48 PI_a.in(x - u_i_a);
btomic 5:d3bdc0672891 49 PI_b.in(x - u_i_b);
btomic 5:d3bdc0672891 50 u_R_a = PI_a.out();
btomic 5:d3bdc0672891 51 u_R_b = PI_b.out();
btomic 5:d3bdc0672891 52
btomic 5:d3bdc0672891 53 motor1.pulsewidth(100e-6*u_R_a);
btomic 5:d3bdc0672891 54 motor2.pulsewidth(100e-6*u_R_b);
btomic 1:50fc6689148e 55
btomic 5:d3bdc0672891 56 }
btomic 1:50fc6689148e 57
btomic 3:fc17c2ad6b81 58 void steerServo(float angleDegrees){
btomic 4:d3c4924179f0 59 float minPw = 0.0005, maxPw = 0.0025, minAng = -90, maxAng = 90, pulse = 0;
btomic 5:d3bdc0672891 60 if (angleDegrees < -29 ) angleDegrees = -29;
btomic 5:d3bdc0672891 61 if (angleDegrees > 30 ) angleDegrees = 30;
btomic 3:fc17c2ad6b81 62 pulse = minPw + ((maxPw - minPw)/(maxAng - minAng))*(angleDegrees - minAng);
btomic 3:fc17c2ad6b81 63 servo.pulsewidth(pulse);
btomic 1:50fc6689148e 64 }
btomic 0:1b75c7a92a5e 65
btomic 0:1b75c7a92a5e 66 int main() {
btomic 5:d3bdc0672891 67 float pw =0, PWmin=-90, PWmax=90;
btomic 1:50fc6689148e 68 float angleDegrees = 0;
btomic 1:50fc6689148e 69 float sampleTime = 50e-3, switchingFrequency = 5000, rampTime = 3;
btomic 2:25b8be806708 70 servo.period(1.0/50);
btomic 5:d3bdc0672891 71 PI_a.setOutputLimits (0.3, 1);
btomic 5:d3bdc0672891 72 PI_b.setOutputLimits (0.3, 1);
btomic 4:d3c4924179f0 73
btomic 4:d3c4924179f0 74 steerServo(angleDegrees);
btomic 5:d3bdc0672891 75 motor1.period_us(100);
btomic 5:d3bdc0672891 76 motor2.period_us(100);
btomic 5:d3bdc0672891 77
btomic 4:d3c4924179f0 78 // motor_a.configure(sampleTime, switchingFrequency, rampTime, rampTime);
btomic 4:d3c4924179f0 79 // motor_b.configure(sampleTime, switchingFrequency, rampTime, rampTime);
btomic 2:25b8be806708 80 wait(5);
btomic 5:d3bdc0672891 81 enable = 1;
btomic 5:d3bdc0672891 82 motor2_grana2 = 0;
btomic 5:d3bdc0672891 83 motor1_grana2 = 0;
btomic 5:d3bdc0672891 84 ticker.attach(&tick, T_d);
btomic 4:d3c4924179f0 85
btomic 0:1b75c7a92a5e 86 while(1){
btomic 5:d3bdc0672891 87
btomic 5:d3bdc0672891 88
btomic 5:d3bdc0672891 89 // angleDegrees = PWmin + (PWmax - PWmin) * pot1;
btomic 5:d3bdc0672891 90
btomic 4:d3c4924179f0 91 // if(sw1==1 && sw2==0) enable = sw1;
btomic 4:d3c4924179f0 92 // else enable = 0;
btomic 5:d3bdc0672891 93
btomic 4:d3c4924179f0 94
btomic 5:d3bdc0672891 95
btomic 1:50fc6689148e 96
btomic 5:d3bdc0672891 97 wait(1);
btomic 5:d3bdc0672891 98
btomic 5:d3bdc0672891 99
btomic 5:d3bdc0672891 100 /** motor2_grana2 = 0;
btomic 4:d3c4924179f0 101
btomic 4:d3c4924179f0 102
btomic 5:d3bdc0672891 103 motor2.pulsewidth_us(100*u_R);
btomic 5:d3bdc0672891 104
btomic 4:d3c4924179f0 105 steerServo(angleDegrees);
btomic 5:d3bdc0672891 106 // pc.printf("pot1: %f angleDegrees: %f pot2: %f\r", x, angleDegrees, u_R);
btomic 1:50fc6689148e 107
btomic 4:d3c4924179f0 108 // motor_a.setDutyCycle(0.2);
btomic 4:d3c4924179f0 109 // motor_b.setDutyCycle(0.2);
btomic 1:50fc6689148e 110
btomic 4:d3c4924179f0 111 if(x<0.501 && x>0.499) {d1=1; d2=1; d3=1; d4=1;}
btomic 4:d3c4924179f0 112 else {d1=1; d2=0; d3=0; d4=1;}
btomic 5:d3bdc0672891 113 **/
btomic 0:1b75c7a92a5e 114 }
btomic 5:d3bdc0672891 115 //enable = 0;
btomic 1:50fc6689148e 116 // red = 1;
btomic 1:50fc6689148e 117
btomic 0:1b75c7a92a5e 118
btomic 0:1b75c7a92a5e 119 }