motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp
- Committer:
- mdraganic
- Date:
- 2013-11-11
- Revision:
- 8:4044ae40bbc1
- Parent:
- 7:ac67f44120a5
- Child:
- 9:764eb7341754
File content as of revision 8:4044ae40bbc1:
#include "mbed.h" #include "Motor.h" #include "TextLCD.h" #define sensorStartTreshold 0.05 #define sensorStopTreshold 0.01 #define calibrateFactorSenzA 1 #define calibrateFactorSenzB 0.97 #define calibrateFactorSenzC 1.06 #define calibrateFactorSenzD 0.9 #define lcdOutputEnable true #define serialOutputEnable false #define serialOutputBaudrate 115200 Serial pc(USBTX, USBRX); TextLCD lcd(p15, p16, p5, p6, p7, p8); // rs, e, d4-d7 DigitalOut ledBoot(LED1); DigitalOut ledAz(LED3); DigitalOut ledEl(LED4); 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); } if(lcdOutputEnable) { lcd.cls(); lcd.printf("a:%5.2f \n", valAzimut); lcd.printf("e:%5.2f \n", valElevacija); } } int main() { ledBoot = 1; pc.baud(serialOutputBaudrate); Motor *motorAz = new Motor(p25, p26, p24); Motor *motorEl = new Motor(p22, p21, p23); wait_ms(500); ledBoot = 0; while(1) { readValuesForAveraging(); // ----- azimut ----- if(abs(valAzimut) > sensorStartTreshold) { if(valAzimut > 0) (*motorAz).movePositive(); else (*motorAz).moveNegative(); ledAz = 1; } if(abs(valAzimut) < sensorStopTreshold) { (*motorAz).stop(); ledAz = 0; } // ----- elevacija ----- if(abs(valElevacija) > sensorStartTreshold) { if(valElevacija > 0) (*motorEl).movePositive(); else (*motorEl).moveNegative(); ledEl = 1; } if(abs(valElevacija) < sensorStopTreshold) { (*motorEl).stop(); ledEl = 0; } // ----- pauza ------ wait_ms(200); } }