motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp@4:03b68322905f, 2013-11-09 (annotated)
- Committer:
- mdraganic
- Date:
- Sat Nov 09 18:35:13 2013 +0000
- Revision:
- 4:03b68322905f
- Parent:
- 3:bebfc64cefe4
- Child:
- 5:7e28f4b64a55
popravljena logika upravljanja motorima
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mdraganic | 0:7447b8021b33 | 1 | #include "mbed.h" |
mdraganic | 0:7447b8021b33 | 2 | #include "Motor.h" |
mdraganic | 3:bebfc64cefe4 | 3 | |
mdraganic | 3:bebfc64cefe4 | 4 | #define sensorStartTreshold 0.07 |
mdraganic | 3:bebfc64cefe4 | 5 | #define sensorStopTreshold 0.02 |
mdraganic | 3:bebfc64cefe4 | 6 | |
mdraganic | 3:bebfc64cefe4 | 7 | #define calibrateFactorSenzA 1 |
mdraganic | 3:bebfc64cefe4 | 8 | #define calibrateFactorSenzB 0.97 |
mdraganic | 3:bebfc64cefe4 | 9 | #define calibrateFactorSenzC 1.06 |
mdraganic | 3:bebfc64cefe4 | 10 | #define calibrateFactorSenzD 0.9 |
mdraganic | 3:bebfc64cefe4 | 11 | |
mdraganic | 4:03b68322905f | 12 | #define serialOutputEnable true |
mdraganic | 4:03b68322905f | 13 | #define serialOutputBaudrate 115200 |
mdraganic | 4:03b68322905f | 14 | |
mdraganic | 3:bebfc64cefe4 | 15 | Serial pc(USBTX, USBRX); |
mdraganic | 0:7447b8021b33 | 16 | |
mdraganic | 0:7447b8021b33 | 17 | AnalogIn ainSensA(p17); |
mdraganic | 0:7447b8021b33 | 18 | AnalogIn ainSensB(p18); |
mdraganic | 0:7447b8021b33 | 19 | AnalogIn ainSensC(p19); |
mdraganic | 0:7447b8021b33 | 20 | AnalogIn ainSensD(p20); |
mdraganic | 0:7447b8021b33 | 21 | |
mdraganic | 2:0bf41ad96558 | 22 | float valAzimut = 0; |
mdraganic | 2:0bf41ad96558 | 23 | float valElevacija = 0; |
mdraganic | 2:0bf41ad96558 | 24 | |
mdraganic | 2:0bf41ad96558 | 25 | float SensA, SensB, SensC, SensD; |
mdraganic | 2:0bf41ad96558 | 26 | void readValuesForAveraging() { |
mdraganic | 2:0bf41ad96558 | 27 | |
mdraganic | 2:0bf41ad96558 | 28 | SensA = 0; |
mdraganic | 2:0bf41ad96558 | 29 | SensB = 0; |
mdraganic | 2:0bf41ad96558 | 30 | SensC = 0; |
mdraganic | 2:0bf41ad96558 | 31 | SensD = 0; |
mdraganic | 2:0bf41ad96558 | 32 | |
mdraganic | 3:bebfc64cefe4 | 33 | for (int i = 0; i < 20; i++) { |
mdraganic | 2:0bf41ad96558 | 34 | SensA += ainSensA; |
mdraganic | 2:0bf41ad96558 | 35 | SensB += ainSensB; |
mdraganic | 2:0bf41ad96558 | 36 | SensC += ainSensC; |
mdraganic | 2:0bf41ad96558 | 37 | SensD += ainSensD; |
mdraganic | 2:0bf41ad96558 | 38 | } |
mdraganic | 3:bebfc64cefe4 | 39 | SensA /= 20; |
mdraganic | 3:bebfc64cefe4 | 40 | SensB /= 20; |
mdraganic | 3:bebfc64cefe4 | 41 | SensC /= 20; |
mdraganic | 3:bebfc64cefe4 | 42 | SensD /= 20; |
mdraganic | 3:bebfc64cefe4 | 43 | |
mdraganic | 3:bebfc64cefe4 | 44 | SensA *= calibrateFactorSenzA; |
mdraganic | 3:bebfc64cefe4 | 45 | SensB *= calibrateFactorSenzB; |
mdraganic | 3:bebfc64cefe4 | 46 | SensC *= calibrateFactorSenzC; |
mdraganic | 3:bebfc64cefe4 | 47 | SensD *= calibrateFactorSenzD; |
mdraganic | 2:0bf41ad96558 | 48 | |
mdraganic | 2:0bf41ad96558 | 49 | valAzimut = (SensA + SensB) - (SensC + SensD); |
mdraganic | 2:0bf41ad96558 | 50 | valElevacija = (SensB + SensC) - (SensA + SensD); |
mdraganic | 3:bebfc64cefe4 | 51 | |
mdraganic | 4:03b68322905f | 52 | if(serialOutputEnable) { |
mdraganic | 4:03b68322905f | 53 | |
mdraganic | 3:bebfc64cefe4 | 54 | // pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n", |
mdraganic | 3:bebfc64cefe4 | 55 | // valAzimut, valElevacija, SensA, SensB, SensC, SensD); |
mdraganic | 4:03b68322905f | 56 | |
mdraganic | 4:03b68322905f | 57 | pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija); |
mdraganic | 4:03b68322905f | 58 | } |
mdraganic | 2:0bf41ad96558 | 59 | } |
mdraganic | 0:7447b8021b33 | 60 | |
mdraganic | 0:7447b8021b33 | 61 | int main() { |
mdraganic | 0:7447b8021b33 | 62 | |
mdraganic | 4:03b68322905f | 63 | pc.baud(serialOutputBaudrate); |
mdraganic | 4:03b68322905f | 64 | |
mdraganic | 3:bebfc64cefe4 | 65 | Motor *motorAz = new Motor(p25, p26, p24); |
mdraganic | 3:bebfc64cefe4 | 66 | Motor *motorEl = new Motor(p22, p21, p23); |
mdraganic | 0:7447b8021b33 | 67 | |
mdraganic | 0:7447b8021b33 | 68 | while(1) { |
mdraganic | 0:7447b8021b33 | 69 | |
mdraganic | 2:0bf41ad96558 | 70 | readValuesForAveraging(); |
mdraganic | 0:7447b8021b33 | 71 | |
mdraganic | 3:bebfc64cefe4 | 72 | // ----- azimut ----- |
mdraganic | 3:bebfc64cefe4 | 73 | |
mdraganic | 4:03b68322905f | 74 | if(abs(valAzimut) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 75 | if(valAzimut > 0) |
mdraganic | 3:bebfc64cefe4 | 76 | (*motorAz).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 77 | else |
mdraganic | 3:bebfc64cefe4 | 78 | (*motorAz).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 79 | } |
mdraganic | 3:bebfc64cefe4 | 80 | |
mdraganic | 4:03b68322905f | 81 | if(abs(valAzimut) < sensorStopTreshold) { |
mdraganic | 3:bebfc64cefe4 | 82 | (*motorAz).stop(); |
mdraganic | 2:0bf41ad96558 | 83 | } |
mdraganic | 3:bebfc64cefe4 | 84 | |
mdraganic | 3:bebfc64cefe4 | 85 | // ----- elevacija ----- |
mdraganic | 3:bebfc64cefe4 | 86 | |
mdraganic | 4:03b68322905f | 87 | if(abs(valElevacija) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 88 | if(valElevacija > 0) |
mdraganic | 3:bebfc64cefe4 | 89 | (*motorEl).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 90 | else |
mdraganic | 3:bebfc64cefe4 | 91 | (*motorEl).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 92 | } |
mdraganic | 3:bebfc64cefe4 | 93 | |
mdraganic | 4:03b68322905f | 94 | if(abs(valElevacija) < sensorStopTreshold) { |
mdraganic | 4:03b68322905f | 95 | (*motorEl).stop(); |
mdraganic | 3:bebfc64cefe4 | 96 | } |
mdraganic | 3:bebfc64cefe4 | 97 | |
mdraganic | 4:03b68322905f | 98 | // ----- pauza ------ |
mdraganic | 4:03b68322905f | 99 | |
mdraganic | 4:03b68322905f | 100 | wait_ms(100); |
mdraganic | 0:7447b8021b33 | 101 | } |
mdraganic | 0:7447b8021b33 | 102 | } |
mdraganic | 2:0bf41ad96558 | 103 | |
mdraganic | 2:0bf41ad96558 | 104 |