motor i senzor
Dependencies: TextLCD mbed PinDetect
Diff: main.cpp
- Revision:
- 2:0bf41ad96558
- Parent:
- 1:3500bf8487d0
- Child:
- 3:bebfc64cefe4
diff -r 3500bf8487d0 -r 0bf41ad96558 main.cpp --- a/main.cpp Sat Nov 09 06:49:43 2013 +0000 +++ b/main.cpp Sat Nov 09 07:04:13 2013 +0000 @@ -7,8 +7,31 @@ AnalogIn ainSensC(p19); AnalogIn ainSensD(p20); -int valAzimut = 0; -int valElevacija = 0; +float valAzimut = 0; +float valElevacija = 0; + +float SensA, SensB, SensC, SensD; +void readValuesForAveraging() { + + SensA = 0; + SensB = 0; + SensC = 0; + SensD = 0; + + for (int i = 0; i <= 9; i++) { + SensA += ainSensA; + SensB += ainSensB; + SensC += ainSensC; + SensD += ainSensD; + } + SensA /= 10; + SensB /= 10; + SensC /= 10; + SensD /= 10; + + valAzimut = (SensA + SensB) - (SensC + SensD); + valElevacija = (SensB + SensC) - (SensA + SensD); +} int main() { @@ -17,15 +40,24 @@ while(1) { - valAzimut = (ainSensA.read_u16() + ainSensB.read_u16()) - - (ainSensC.read_u16() + ainSensD.read_u16()); - valElevacija = (ainSensB.read_u16() + ainSensC.read_u16()) - - (ainSensA.read_u16() + ainSensD.read_u16()); - - (*motorEl).stop(); - (*motorEl).movePositive(); - (*motorEl).moveNegative(); + readValuesForAveraging(); - + if (valAzimut > 0.2){ // positive azimuth deviation + (*motorAz).movePositive(); + } + else if (valAzimut < -0.2) { // negative azimuth deviation + (*motorAz).moveNegative(); + } + (*motorAz).stop(); + + if (valElevacija > 0.2){ // positive azimuth deviation + (*motorEl).movePositive(); + } + else if (valElevacija < -0.2) { // negative azimuth deviation + (*motorEl).moveNegative(); + } + (*motorEl).stop(); } } + +