motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp
- Committer:
- cvitas
- Date:
- 2013-12-11
- Revision:
- 11:f243d8ed556d
- Parent:
- 10:0b8a98b1e6b1
- Child:
- 12:8cb20afa1694
File content as of revision 11:f243d8ed556d:
#include "mbed.h" #include "Motor.h" #include "TextLCD.h" #include "PinDetect.h" #define sensorStartTreshold 0.08 #define sensorStopTreshold 0.02 #define calibrateFactorSenzA 1 #define calibrateFactorSenzB 0.97 #define calibrateFactorSenzC 1.06 #define calibrateFactorSenzD 0.9 #define lcdOutputEnable true #define serialOutputEnable true #define serialOutputBaudrate 9600 Serial pc(USBTX, USBRX); TextLCD lcd(p13, p14, p5, p6, p7, p8); // rs, e, d4-d7 Ticker Prikaz; DigitalOut ledBoot(LED1); DigitalOut ledAz(LED3); DigitalOut ledEl(LED4); AnalogIn ainSensA(p16); AnalogIn ainSensB(p17); AnalogIn ainSensC(p18); AnalogIn ainSensD(p19); PinDetect pb_azimut(p11); PinDetect pb_elevacija(p12); float valAzimut = 0; float valElevacija = 0; int volatile count_azimut=0; int volatile count_elevacija=0; float SensA, SensB, SensC, SensD; // Callback routine is interrupt activated by a debounced pb hit void pb_azimut_hit_callback (void) { count_azimut++; } void pb_elevacija_hit_callback (void) { count_elevacija++; } char GetKeyInput(void) { char c = pc.getc(); // get keyboard data (note numerical ascii range) pc.printf("%c",c); // print ascii value to host PC terminal return (c&0x0F); // return value as non-ascii (bitmask c with value 0x0f } void od_prikaza(){ lcd.cls(); lcd.printf(" %d %d ", count_azimut , count_elevacija); } 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\r", 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); } } char data1, data2; // variable declarations int main() { ledBoot = 1; Prikaz.attach(&od_prikaza, 0.1); // Setup Interrupt callback function for a pb hit pb_elevacija.attach_deasserted(&pb_elevacija_hit_callback); pb_azimut.attach_deasserted(&pb_azimut_hit_callback); // Start sampling pb input using interrupts pb_elevacija.setSampleFrequency(); pb_azimut.setSampleFrequency(); pc.baud(serialOutputBaudrate); Motor *motorAz = new Motor(p28, p27, p26); // relej, relej, pwm Motor *motorEl = new Motor(p24, p23, p25); if(lcdOutputEnable) { lcd.cls(); lcd.printf("Suncokre\n"); lcd.printf("t TVZ \n"); } wait_ms(5000); ledBoot = 0; while(1) { data2 = GetKeyInput(); // call function to get 1st key press switch (data2) { case 0: (*motorAz).stop(); (*motorEl).stop(); break; case 1: (*motorAz).movePositive(); break; case 2: (*motorAz).moveNegative(); break; case 3: (*motorEl).movePositive(); break; case 4: (*motorEl).moveNegative(); break; default: (*motorAz).stop(); (*motorEl).stop(); break; } /* // ---- citanje senzora ------------ 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); } }