motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp@8:4044ae40bbc1, 2013-11-11 (annotated)
- Committer:
- mdraganic
- Date:
- Mon Nov 11 13:26:06 2013 +0000
- Revision:
- 8:4044ae40bbc1
- Parent:
- 7:ac67f44120a5
- Child:
- 9:764eb7341754
LCD gotov
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 | 7:ac67f44120a5 | 3 | #include "TextLCD.h" |
mdraganic | 3:bebfc64cefe4 | 4 | |
mdraganic | 6:902bec57d9ae | 5 | #define sensorStartTreshold 0.05 |
mdraganic | 6:902bec57d9ae | 6 | #define sensorStopTreshold 0.01 |
mdraganic | 3:bebfc64cefe4 | 7 | |
mdraganic | 5:7e28f4b64a55 | 8 | #define calibrateFactorSenzA 1 |
mdraganic | 5:7e28f4b64a55 | 9 | #define calibrateFactorSenzB 0.97 |
mdraganic | 5:7e28f4b64a55 | 10 | #define calibrateFactorSenzC 1.06 |
mdraganic | 5:7e28f4b64a55 | 11 | #define calibrateFactorSenzD 0.9 |
mdraganic | 3:bebfc64cefe4 | 12 | |
mdraganic | 7:ac67f44120a5 | 13 | #define lcdOutputEnable true |
mdraganic | 7:ac67f44120a5 | 14 | #define serialOutputEnable false |
mdraganic | 4:03b68322905f | 15 | #define serialOutputBaudrate 115200 |
mdraganic | 4:03b68322905f | 16 | |
mdraganic | 3:bebfc64cefe4 | 17 | Serial pc(USBTX, USBRX); |
mdraganic | 7:ac67f44120a5 | 18 | TextLCD lcd(p15, p16, p5, p6, p7, p8); // rs, e, d4-d7 |
mdraganic | 7:ac67f44120a5 | 19 | |
mdraganic | 7:ac67f44120a5 | 20 | DigitalOut ledBoot(LED1); |
mdraganic | 7:ac67f44120a5 | 21 | DigitalOut ledAz(LED3); |
mdraganic | 7:ac67f44120a5 | 22 | DigitalOut ledEl(LED4); |
mdraganic | 0:7447b8021b33 | 23 | |
mdraganic | 0:7447b8021b33 | 24 | AnalogIn ainSensA(p17); |
mdraganic | 0:7447b8021b33 | 25 | AnalogIn ainSensB(p18); |
mdraganic | 0:7447b8021b33 | 26 | AnalogIn ainSensC(p19); |
mdraganic | 0:7447b8021b33 | 27 | AnalogIn ainSensD(p20); |
mdraganic | 0:7447b8021b33 | 28 | |
mdraganic | 2:0bf41ad96558 | 29 | float valAzimut = 0; |
mdraganic | 2:0bf41ad96558 | 30 | float valElevacija = 0; |
mdraganic | 2:0bf41ad96558 | 31 | |
mdraganic | 2:0bf41ad96558 | 32 | float SensA, SensB, SensC, SensD; |
mdraganic | 2:0bf41ad96558 | 33 | void readValuesForAveraging() { |
mdraganic | 2:0bf41ad96558 | 34 | |
mdraganic | 2:0bf41ad96558 | 35 | SensA = 0; |
mdraganic | 2:0bf41ad96558 | 36 | SensB = 0; |
mdraganic | 2:0bf41ad96558 | 37 | SensC = 0; |
mdraganic | 2:0bf41ad96558 | 38 | SensD = 0; |
mdraganic | 2:0bf41ad96558 | 39 | |
mdraganic | 3:bebfc64cefe4 | 40 | for (int i = 0; i < 20; i++) { |
mdraganic | 2:0bf41ad96558 | 41 | SensA += ainSensA; |
mdraganic | 2:0bf41ad96558 | 42 | SensB += ainSensB; |
mdraganic | 2:0bf41ad96558 | 43 | SensC += ainSensC; |
mdraganic | 2:0bf41ad96558 | 44 | SensD += ainSensD; |
mdraganic | 2:0bf41ad96558 | 45 | } |
mdraganic | 3:bebfc64cefe4 | 46 | SensA /= 20; |
mdraganic | 3:bebfc64cefe4 | 47 | SensB /= 20; |
mdraganic | 3:bebfc64cefe4 | 48 | SensC /= 20; |
mdraganic | 3:bebfc64cefe4 | 49 | SensD /= 20; |
mdraganic | 3:bebfc64cefe4 | 50 | |
mdraganic | 3:bebfc64cefe4 | 51 | SensA *= calibrateFactorSenzA; |
mdraganic | 3:bebfc64cefe4 | 52 | SensB *= calibrateFactorSenzB; |
mdraganic | 3:bebfc64cefe4 | 53 | SensC *= calibrateFactorSenzC; |
mdraganic | 3:bebfc64cefe4 | 54 | SensD *= calibrateFactorSenzD; |
mdraganic | 2:0bf41ad96558 | 55 | |
mdraganic | 2:0bf41ad96558 | 56 | valAzimut = (SensA + SensB) - (SensC + SensD); |
mdraganic | 2:0bf41ad96558 | 57 | valElevacija = (SensB + SensC) - (SensA + SensD); |
mdraganic | 3:bebfc64cefe4 | 58 | |
mdraganic | 4:03b68322905f | 59 | if(serialOutputEnable) { |
mdraganic | 4:03b68322905f | 60 | |
mdraganic | 3:bebfc64cefe4 | 61 | // pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n", |
mdraganic | 3:bebfc64cefe4 | 62 | // valAzimut, valElevacija, SensA, SensB, SensC, SensD); |
mdraganic | 4:03b68322905f | 63 | |
mdraganic | 4:03b68322905f | 64 | pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija); |
mdraganic | 4:03b68322905f | 65 | } |
mdraganic | 7:ac67f44120a5 | 66 | |
mdraganic | 7:ac67f44120a5 | 67 | if(lcdOutputEnable) { |
mdraganic | 7:ac67f44120a5 | 68 | lcd.cls(); |
mdraganic | 8:4044ae40bbc1 | 69 | lcd.printf("a:%5.2f \n", valAzimut); |
mdraganic | 8:4044ae40bbc1 | 70 | lcd.printf("e:%5.2f \n", valElevacija); |
mdraganic | 7:ac67f44120a5 | 71 | } |
mdraganic | 7:ac67f44120a5 | 72 | |
mdraganic | 2:0bf41ad96558 | 73 | } |
mdraganic | 0:7447b8021b33 | 74 | |
mdraganic | 0:7447b8021b33 | 75 | int main() { |
mdraganic | 0:7447b8021b33 | 76 | |
mdraganic | 7:ac67f44120a5 | 77 | ledBoot = 1; |
mdraganic | 4:03b68322905f | 78 | pc.baud(serialOutputBaudrate); |
mdraganic | 3:bebfc64cefe4 | 79 | Motor *motorAz = new Motor(p25, p26, p24); |
mdraganic | 3:bebfc64cefe4 | 80 | Motor *motorEl = new Motor(p22, p21, p23); |
mdraganic | 7:ac67f44120a5 | 81 | wait_ms(500); |
mdraganic | 7:ac67f44120a5 | 82 | ledBoot = 0; |
mdraganic | 0:7447b8021b33 | 83 | |
mdraganic | 0:7447b8021b33 | 84 | while(1) { |
mdraganic | 7:ac67f44120a5 | 85 | |
mdraganic | 2:0bf41ad96558 | 86 | readValuesForAveraging(); |
mdraganic | 0:7447b8021b33 | 87 | |
mdraganic | 3:bebfc64cefe4 | 88 | // ----- azimut ----- |
mdraganic | 3:bebfc64cefe4 | 89 | |
mdraganic | 4:03b68322905f | 90 | if(abs(valAzimut) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 91 | if(valAzimut > 0) |
mdraganic | 3:bebfc64cefe4 | 92 | (*motorAz).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 93 | else |
mdraganic | 3:bebfc64cefe4 | 94 | (*motorAz).moveNegative(); |
mdraganic | 7:ac67f44120a5 | 95 | ledAz = 1; |
mdraganic | 3:bebfc64cefe4 | 96 | } |
mdraganic | 3:bebfc64cefe4 | 97 | |
mdraganic | 4:03b68322905f | 98 | if(abs(valAzimut) < sensorStopTreshold) { |
mdraganic | 3:bebfc64cefe4 | 99 | (*motorAz).stop(); |
mdraganic | 7:ac67f44120a5 | 100 | ledAz = 0; |
mdraganic | 2:0bf41ad96558 | 101 | } |
mdraganic | 3:bebfc64cefe4 | 102 | |
mdraganic | 3:bebfc64cefe4 | 103 | // ----- elevacija ----- |
mdraganic | 3:bebfc64cefe4 | 104 | |
mdraganic | 4:03b68322905f | 105 | if(abs(valElevacija) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 106 | if(valElevacija > 0) |
mdraganic | 3:bebfc64cefe4 | 107 | (*motorEl).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 108 | else |
mdraganic | 3:bebfc64cefe4 | 109 | (*motorEl).moveNegative(); |
mdraganic | 7:ac67f44120a5 | 110 | ledEl = 1; |
mdraganic | 3:bebfc64cefe4 | 111 | } |
mdraganic | 3:bebfc64cefe4 | 112 | |
mdraganic | 4:03b68322905f | 113 | if(abs(valElevacija) < sensorStopTreshold) { |
mdraganic | 4:03b68322905f | 114 | (*motorEl).stop(); |
mdraganic | 7:ac67f44120a5 | 115 | ledEl = 0; |
mdraganic | 3:bebfc64cefe4 | 116 | } |
mdraganic | 3:bebfc64cefe4 | 117 | |
mdraganic | 4:03b68322905f | 118 | // ----- pauza ------ |
mdraganic | 4:03b68322905f | 119 | |
mdraganic | 7:ac67f44120a5 | 120 | wait_ms(200); |
mdraganic | 0:7447b8021b33 | 121 | } |
mdraganic | 0:7447b8021b33 | 122 | } |
mdraganic | 2:0bf41ad96558 | 123 | |
mdraganic | 2:0bf41ad96558 | 124 |