motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp@6:902bec57d9ae, 2013-11-11 (annotated)
- Committer:
- mdraganic
- Date:
- Mon Nov 11 11:45:55 2013 +0000
- Revision:
- 6:902bec57d9ae
- Parent:
- 5:7e28f4b64a55
- Child:
- 7:ac67f44120a5
gotov driver i tracker
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 | 6:902bec57d9ae | 4 | #define sensorStartTreshold 0.05 |
mdraganic | 6:902bec57d9ae | 5 | #define sensorStopTreshold 0.01 |
mdraganic | 3:bebfc64cefe4 | 6 | |
mdraganic | 5:7e28f4b64a55 | 7 | #define calibrateFactorSenzA 1 |
mdraganic | 5:7e28f4b64a55 | 8 | #define calibrateFactorSenzB 0.97 |
mdraganic | 5:7e28f4b64a55 | 9 | #define calibrateFactorSenzC 1.06 |
mdraganic | 5:7e28f4b64a55 | 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 | 6:902bec57d9ae | 70 | (*motorEl).movePositive(); |
mdraganic | 6:902bec57d9ae | 71 | (*motorAz).movePositive(); |
mdraganic | 6:902bec57d9ae | 72 | wait(5); |
mdraganic | 6:902bec57d9ae | 73 | (*motorAz).moveNegative(); |
mdraganic | 6:902bec57d9ae | 74 | (*motorEl).moveNegative(); |
mdraganic | 6:902bec57d9ae | 75 | wait(3); |
mdraganic | 6:902bec57d9ae | 76 | continue; |
mdraganic | 6:902bec57d9ae | 77 | |
mdraganic | 6:902bec57d9ae | 78 | |
mdraganic | 6:902bec57d9ae | 79 | |
mdraganic | 2:0bf41ad96558 | 80 | readValuesForAveraging(); |
mdraganic | 0:7447b8021b33 | 81 | |
mdraganic | 3:bebfc64cefe4 | 82 | // ----- azimut ----- |
mdraganic | 3:bebfc64cefe4 | 83 | |
mdraganic | 4:03b68322905f | 84 | if(abs(valAzimut) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 85 | if(valAzimut > 0) |
mdraganic | 3:bebfc64cefe4 | 86 | (*motorAz).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 87 | else |
mdraganic | 3:bebfc64cefe4 | 88 | (*motorAz).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 89 | } |
mdraganic | 3:bebfc64cefe4 | 90 | |
mdraganic | 4:03b68322905f | 91 | if(abs(valAzimut) < sensorStopTreshold) { |
mdraganic | 3:bebfc64cefe4 | 92 | (*motorAz).stop(); |
mdraganic | 2:0bf41ad96558 | 93 | } |
mdraganic | 3:bebfc64cefe4 | 94 | |
mdraganic | 3:bebfc64cefe4 | 95 | // ----- elevacija ----- |
mdraganic | 3:bebfc64cefe4 | 96 | |
mdraganic | 4:03b68322905f | 97 | if(abs(valElevacija) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 98 | if(valElevacija > 0) |
mdraganic | 3:bebfc64cefe4 | 99 | (*motorEl).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 100 | else |
mdraganic | 3:bebfc64cefe4 | 101 | (*motorEl).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 102 | } |
mdraganic | 3:bebfc64cefe4 | 103 | |
mdraganic | 4:03b68322905f | 104 | if(abs(valElevacija) < sensorStopTreshold) { |
mdraganic | 4:03b68322905f | 105 | (*motorEl).stop(); |
mdraganic | 3:bebfc64cefe4 | 106 | } |
mdraganic | 3:bebfc64cefe4 | 107 | |
mdraganic | 4:03b68322905f | 108 | // ----- pauza ------ |
mdraganic | 4:03b68322905f | 109 | |
mdraganic | 4:03b68322905f | 110 | wait_ms(100); |
mdraganic | 0:7447b8021b33 | 111 | } |
mdraganic | 0:7447b8021b33 | 112 | } |
mdraganic | 2:0bf41ad96558 | 113 | |
mdraganic | 2:0bf41ad96558 | 114 |