motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp@2:0bf41ad96558, 2013-11-09 (annotated)
- Committer:
- mdraganic
- Date:
- Sat Nov 09 07:04:13 2013 +0000
- Revision:
- 2:0bf41ad96558
- Parent:
- 1:3500bf8487d0
- Child:
- 3:bebfc64cefe4
added sensor averageing
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 | 1:3500bf8487d0 | 3 | #include "MotorPwm.h" |
mdraganic | 0:7447b8021b33 | 4 | |
mdraganic | 0:7447b8021b33 | 5 | AnalogIn ainSensA(p17); |
mdraganic | 0:7447b8021b33 | 6 | AnalogIn ainSensB(p18); |
mdraganic | 0:7447b8021b33 | 7 | AnalogIn ainSensC(p19); |
mdraganic | 0:7447b8021b33 | 8 | AnalogIn ainSensD(p20); |
mdraganic | 0:7447b8021b33 | 9 | |
mdraganic | 2:0bf41ad96558 | 10 | float valAzimut = 0; |
mdraganic | 2:0bf41ad96558 | 11 | float valElevacija = 0; |
mdraganic | 2:0bf41ad96558 | 12 | |
mdraganic | 2:0bf41ad96558 | 13 | float SensA, SensB, SensC, SensD; |
mdraganic | 2:0bf41ad96558 | 14 | void readValuesForAveraging() { |
mdraganic | 2:0bf41ad96558 | 15 | |
mdraganic | 2:0bf41ad96558 | 16 | SensA = 0; |
mdraganic | 2:0bf41ad96558 | 17 | SensB = 0; |
mdraganic | 2:0bf41ad96558 | 18 | SensC = 0; |
mdraganic | 2:0bf41ad96558 | 19 | SensD = 0; |
mdraganic | 2:0bf41ad96558 | 20 | |
mdraganic | 2:0bf41ad96558 | 21 | for (int i = 0; i <= 9; i++) { |
mdraganic | 2:0bf41ad96558 | 22 | SensA += ainSensA; |
mdraganic | 2:0bf41ad96558 | 23 | SensB += ainSensB; |
mdraganic | 2:0bf41ad96558 | 24 | SensC += ainSensC; |
mdraganic | 2:0bf41ad96558 | 25 | SensD += ainSensD; |
mdraganic | 2:0bf41ad96558 | 26 | } |
mdraganic | 2:0bf41ad96558 | 27 | SensA /= 10; |
mdraganic | 2:0bf41ad96558 | 28 | SensB /= 10; |
mdraganic | 2:0bf41ad96558 | 29 | SensC /= 10; |
mdraganic | 2:0bf41ad96558 | 30 | SensD /= 10; |
mdraganic | 2:0bf41ad96558 | 31 | |
mdraganic | 2:0bf41ad96558 | 32 | valAzimut = (SensA + SensB) - (SensC + SensD); |
mdraganic | 2:0bf41ad96558 | 33 | valElevacija = (SensB + SensC) - (SensA + SensD); |
mdraganic | 2:0bf41ad96558 | 34 | } |
mdraganic | 0:7447b8021b33 | 35 | |
mdraganic | 0:7447b8021b33 | 36 | int main() { |
mdraganic | 0:7447b8021b33 | 37 | |
mdraganic | 1:3500bf8487d0 | 38 | Motor *motorEl = new MotorPwm(p25, p26); |
mdraganic | 1:3500bf8487d0 | 39 | Motor *motorAz = new MotorPwm(p23, p24); |
mdraganic | 0:7447b8021b33 | 40 | |
mdraganic | 0:7447b8021b33 | 41 | while(1) { |
mdraganic | 0:7447b8021b33 | 42 | |
mdraganic | 2:0bf41ad96558 | 43 | readValuesForAveraging(); |
mdraganic | 0:7447b8021b33 | 44 | |
mdraganic | 2:0bf41ad96558 | 45 | if (valAzimut > 0.2){ // positive azimuth deviation |
mdraganic | 2:0bf41ad96558 | 46 | (*motorAz).movePositive(); |
mdraganic | 2:0bf41ad96558 | 47 | } |
mdraganic | 2:0bf41ad96558 | 48 | else if (valAzimut < -0.2) { // negative azimuth deviation |
mdraganic | 2:0bf41ad96558 | 49 | (*motorAz).moveNegative(); |
mdraganic | 2:0bf41ad96558 | 50 | } |
mdraganic | 2:0bf41ad96558 | 51 | (*motorAz).stop(); |
mdraganic | 2:0bf41ad96558 | 52 | |
mdraganic | 2:0bf41ad96558 | 53 | if (valElevacija > 0.2){ // positive azimuth deviation |
mdraganic | 2:0bf41ad96558 | 54 | (*motorEl).movePositive(); |
mdraganic | 2:0bf41ad96558 | 55 | } |
mdraganic | 2:0bf41ad96558 | 56 | else if (valElevacija < -0.2) { // negative azimuth deviation |
mdraganic | 2:0bf41ad96558 | 57 | (*motorEl).moveNegative(); |
mdraganic | 2:0bf41ad96558 | 58 | } |
mdraganic | 2:0bf41ad96558 | 59 | (*motorEl).stop(); |
mdraganic | 0:7447b8021b33 | 60 | } |
mdraganic | 0:7447b8021b33 | 61 | } |
mdraganic | 2:0bf41ad96558 | 62 | |
mdraganic | 2:0bf41ad96558 | 63 |