motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp
- Committer:
- mdraganic
- Date:
- 2013-11-09
- Revision:
- 4:03b68322905f
- Parent:
- 3:bebfc64cefe4
- Child:
- 5:7e28f4b64a55
File content as of revision 4:03b68322905f:
#include "mbed.h" #include "Motor.h" #define sensorStartTreshold 0.07 #define sensorStopTreshold 0.02 #define calibrateFactorSenzA 1 #define calibrateFactorSenzB 0.97 #define calibrateFactorSenzC 1.06 #define calibrateFactorSenzD 0.9 #define serialOutputEnable true #define serialOutputBaudrate 115200 Serial pc(USBTX, USBRX); AnalogIn ainSensA(p17); AnalogIn ainSensB(p18); AnalogIn ainSensC(p19); AnalogIn ainSensD(p20); 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 < 20; i++) { SensA += ainSensA; SensB += ainSensB; SensC += ainSensC; SensD += ainSensD; } SensA /= 20; SensB /= 20; SensC /= 20; SensD /= 20; SensA *= calibrateFactorSenzA; SensB *= calibrateFactorSenzB; SensC *= calibrateFactorSenzC; SensD *= calibrateFactorSenzD; valAzimut = (SensA + SensB) - (SensC + SensD); valElevacija = (SensB + SensC) - (SensA + SensD); if(serialOutputEnable) { // pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n", // valAzimut, valElevacija, SensA, SensB, SensC, SensD); pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija); } } int main() { pc.baud(serialOutputBaudrate); Motor *motorAz = new Motor(p25, p26, p24); Motor *motorEl = new Motor(p22, p21, p23); while(1) { readValuesForAveraging(); // ----- azimut ----- if(abs(valAzimut) > sensorStartTreshold) { if(valAzimut > 0) (*motorAz).movePositive(); else (*motorAz).moveNegative(); } if(abs(valAzimut) < sensorStopTreshold) { (*motorAz).stop(); } // ----- elevacija ----- if(abs(valElevacija) > sensorStartTreshold) { if(valElevacija > 0) (*motorEl).movePositive(); else (*motorEl).moveNegative(); } if(abs(valElevacija) < sensorStopTreshold) { (*motorEl).stop(); } // ----- pauza ------ wait_ms(100); } }