motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp@3:bebfc64cefe4, 2013-11-09 (annotated)
- Committer:
- mdraganic
- Date:
- Sat Nov 09 13:44:18 2013 +0000
- Revision:
- 3:bebfc64cefe4
- Parent:
- 2:0bf41ad96558
- Child:
- 4:03b68322905f
sve skupa i zajedno
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 | 3:bebfc64cefe4 | 12 | Serial pc(USBTX, USBRX); |
mdraganic | 0:7447b8021b33 | 13 | |
mdraganic | 0:7447b8021b33 | 14 | AnalogIn ainSensA(p17); |
mdraganic | 0:7447b8021b33 | 15 | AnalogIn ainSensB(p18); |
mdraganic | 0:7447b8021b33 | 16 | AnalogIn ainSensC(p19); |
mdraganic | 0:7447b8021b33 | 17 | AnalogIn ainSensD(p20); |
mdraganic | 0:7447b8021b33 | 18 | |
mdraganic | 2:0bf41ad96558 | 19 | float valAzimut = 0; |
mdraganic | 2:0bf41ad96558 | 20 | float valElevacija = 0; |
mdraganic | 2:0bf41ad96558 | 21 | |
mdraganic | 2:0bf41ad96558 | 22 | float SensA, SensB, SensC, SensD; |
mdraganic | 2:0bf41ad96558 | 23 | void readValuesForAveraging() { |
mdraganic | 2:0bf41ad96558 | 24 | |
mdraganic | 2:0bf41ad96558 | 25 | SensA = 0; |
mdraganic | 2:0bf41ad96558 | 26 | SensB = 0; |
mdraganic | 2:0bf41ad96558 | 27 | SensC = 0; |
mdraganic | 2:0bf41ad96558 | 28 | SensD = 0; |
mdraganic | 2:0bf41ad96558 | 29 | |
mdraganic | 3:bebfc64cefe4 | 30 | for (int i = 0; i < 20; i++) { |
mdraganic | 2:0bf41ad96558 | 31 | SensA += ainSensA; |
mdraganic | 2:0bf41ad96558 | 32 | SensB += ainSensB; |
mdraganic | 2:0bf41ad96558 | 33 | SensC += ainSensC; |
mdraganic | 2:0bf41ad96558 | 34 | SensD += ainSensD; |
mdraganic | 2:0bf41ad96558 | 35 | } |
mdraganic | 3:bebfc64cefe4 | 36 | SensA /= 20; |
mdraganic | 3:bebfc64cefe4 | 37 | SensB /= 20; |
mdraganic | 3:bebfc64cefe4 | 38 | SensC /= 20; |
mdraganic | 3:bebfc64cefe4 | 39 | SensD /= 20; |
mdraganic | 3:bebfc64cefe4 | 40 | |
mdraganic | 3:bebfc64cefe4 | 41 | SensA *= calibrateFactorSenzA; |
mdraganic | 3:bebfc64cefe4 | 42 | SensB *= calibrateFactorSenzB; |
mdraganic | 3:bebfc64cefe4 | 43 | SensC *= calibrateFactorSenzC; |
mdraganic | 3:bebfc64cefe4 | 44 | SensD *= calibrateFactorSenzD; |
mdraganic | 2:0bf41ad96558 | 45 | |
mdraganic | 2:0bf41ad96558 | 46 | valAzimut = (SensA + SensB) - (SensC + SensD); |
mdraganic | 2:0bf41ad96558 | 47 | valElevacija = (SensB + SensC) - (SensA + SensD); |
mdraganic | 3:bebfc64cefe4 | 48 | |
mdraganic | 3:bebfc64cefe4 | 49 | // pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n", |
mdraganic | 3:bebfc64cefe4 | 50 | // valAzimut, valElevacija, SensA, SensB, SensC, SensD); |
mdraganic | 3:bebfc64cefe4 | 51 | pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija); |
mdraganic | 2:0bf41ad96558 | 52 | } |
mdraganic | 0:7447b8021b33 | 53 | |
mdraganic | 0:7447b8021b33 | 54 | int main() { |
mdraganic | 0:7447b8021b33 | 55 | |
mdraganic | 3:bebfc64cefe4 | 56 | Motor *motorAz = new Motor(p25, p26, p24); |
mdraganic | 3:bebfc64cefe4 | 57 | Motor *motorEl = new Motor(p22, p21, p23); |
mdraganic | 0:7447b8021b33 | 58 | |
mdraganic | 0:7447b8021b33 | 59 | while(1) { |
mdraganic | 0:7447b8021b33 | 60 | |
mdraganic | 2:0bf41ad96558 | 61 | readValuesForAveraging(); |
mdraganic | 0:7447b8021b33 | 62 | |
mdraganic | 3:bebfc64cefe4 | 63 | bool someWrokDone = false; |
mdraganic | 3:bebfc64cefe4 | 64 | |
mdraganic | 3:bebfc64cefe4 | 65 | // ----- azimut ----- |
mdraganic | 3:bebfc64cefe4 | 66 | |
mdraganic | 3:bebfc64cefe4 | 67 | if(!(*motorAz).isMoving() && (abs(valAzimut) > sensorStartTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 68 | if(valAzimut > 0) |
mdraganic | 3:bebfc64cefe4 | 69 | (*motorAz).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 70 | else |
mdraganic | 3:bebfc64cefe4 | 71 | (*motorAz).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 72 | someWrokDone = true; |
mdraganic | 3:bebfc64cefe4 | 73 | } |
mdraganic | 3:bebfc64cefe4 | 74 | |
mdraganic | 3:bebfc64cefe4 | 75 | if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 76 | (*motorAz).stop(); |
mdraganic | 3:bebfc64cefe4 | 77 | someWrokDone = true; |
mdraganic | 2:0bf41ad96558 | 78 | } |
mdraganic | 3:bebfc64cefe4 | 79 | |
mdraganic | 3:bebfc64cefe4 | 80 | // ----- elevacija ----- |
mdraganic | 3:bebfc64cefe4 | 81 | |
mdraganic | 3:bebfc64cefe4 | 82 | if(!(*motorEl).isMoving() && (abs(valElevacija) > sensorStartTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 83 | if(valElevacija > 0) |
mdraganic | 3:bebfc64cefe4 | 84 | (*motorEl).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 85 | else |
mdraganic | 3:bebfc64cefe4 | 86 | (*motorEl).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 87 | someWrokDone = true; |
mdraganic | 2:0bf41ad96558 | 88 | } |
mdraganic | 3:bebfc64cefe4 | 89 | |
mdraganic | 3:bebfc64cefe4 | 90 | if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 91 | (*motorEl).stop(); |
mdraganic | 3:bebfc64cefe4 | 92 | someWrokDone = true; |
mdraganic | 3:bebfc64cefe4 | 93 | } |
mdraganic | 3:bebfc64cefe4 | 94 | |
mdraganic | 3:bebfc64cefe4 | 95 | // ---- ništa nije dirano ---- |
mdraganic | 3:bebfc64cefe4 | 96 | |
mdraganic | 3:bebfc64cefe4 | 97 | if(!someWrokDone) |
mdraganic | 3:bebfc64cefe4 | 98 | wait_ms(500); |
mdraganic | 3:bebfc64cefe4 | 99 | |
mdraganic | 3:bebfc64cefe4 | 100 | |
mdraganic | 3:bebfc64cefe4 | 101 | |
mdraganic | 2:0bf41ad96558 | 102 | |
mdraganic | 3:bebfc64cefe4 | 103 | /* |
mdraganic | 3:bebfc64cefe4 | 104 | if(valAzimut > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 105 | if(!(*motorAz).isMoving()) |
mdraganic | 3:bebfc64cefe4 | 106 | (*motorAz).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 107 | continue; |
mdraganic | 3:bebfc64cefe4 | 108 | } |
mdraganic | 3:bebfc64cefe4 | 109 | |
mdraganic | 3:bebfc64cefe4 | 110 | if(valAzimut < (-1 * sensorStartTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 111 | if(!(*motorAz).isMoving()) |
mdraganic | 3:bebfc64cefe4 | 112 | (*motorAz).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 113 | continue; |
mdraganic | 3:bebfc64cefe4 | 114 | } |
mdraganic | 3:bebfc64cefe4 | 115 | |
mdraganic | 3:bebfc64cefe4 | 116 | if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 117 | (*motorAz).stop(); |
mdraganic | 3:bebfc64cefe4 | 118 | } |
mdraganic | 3:bebfc64cefe4 | 119 | |
mdraganic | 3:bebfc64cefe4 | 120 | if(valElevacija > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 121 | if(!(*motorEl).isMoving()) |
mdraganic | 3:bebfc64cefe4 | 122 | (*motorEl).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 123 | continue; |
mdraganic | 2:0bf41ad96558 | 124 | } |
mdraganic | 3:bebfc64cefe4 | 125 | |
mdraganic | 3:bebfc64cefe4 | 126 | if(valElevacija < (-1 * sensorStartTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 127 | if(!(*motorEl).isMoving()) |
mdraganic | 3:bebfc64cefe4 | 128 | (*motorEl).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 129 | continue; |
mdraganic | 3:bebfc64cefe4 | 130 | } |
mdraganic | 3:bebfc64cefe4 | 131 | |
mdraganic | 3:bebfc64cefe4 | 132 | if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) { |
mdraganic | 3:bebfc64cefe4 | 133 | (*motorEl).stop(); |
mdraganic | 3:bebfc64cefe4 | 134 | } |
mdraganic | 3:bebfc64cefe4 | 135 | */ |
mdraganic | 3:bebfc64cefe4 | 136 | |
mdraganic | 3:bebfc64cefe4 | 137 | /* |
mdraganic | 3:bebfc64cefe4 | 138 | if (valElevacija > sensorTreshold){ // positive azimuth deviation |
mdraganic | 3:bebfc64cefe4 | 139 | (*motorEl).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 140 | pc.printf("elevacija pozitiv \n"); |
mdraganic | 3:bebfc64cefe4 | 141 | } |
mdraganic | 3:bebfc64cefe4 | 142 | else if (valElevacija < (-1 * sensorTreshold)) { // negative azimuth deviation |
mdraganic | 2:0bf41ad96558 | 143 | (*motorEl).moveNegative(); |
mdraganic | 3:bebfc64cefe4 | 144 | pc.printf("elevacija negativ \n"); |
mdraganic | 2:0bf41ad96558 | 145 | } |
mdraganic | 3:bebfc64cefe4 | 146 | */ |
mdraganic | 0:7447b8021b33 | 147 | } |
mdraganic | 0:7447b8021b33 | 148 | } |
mdraganic | 2:0bf41ad96558 | 149 | |
mdraganic | 2:0bf41ad96558 | 150 |