motor i senzor
Dependencies: TextLCD mbed PinDetect
main.cpp@11:f243d8ed556d, 2013-12-11 (annotated)
- Committer:
- cvitas
- Date:
- Wed Dec 11 14:54:25 2013 +0000
- Revision:
- 11:f243d8ed556d
- Parent:
- 10:0b8a98b1e6b1
- Child:
- 12:8cb20afa1694
Verzija za kalibraciju platna 1
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" |
cvitas | 11:f243d8ed556d | 4 | #include "PinDetect.h" |
mdraganic | 3:bebfc64cefe4 | 5 | |
mdraganic | 10:0b8a98b1e6b1 | 6 | #define sensorStartTreshold 0.08 |
mdraganic | 10:0b8a98b1e6b1 | 7 | #define sensorStopTreshold 0.02 |
mdraganic | 3:bebfc64cefe4 | 8 | |
mdraganic | 5:7e28f4b64a55 | 9 | #define calibrateFactorSenzA 1 |
mdraganic | 5:7e28f4b64a55 | 10 | #define calibrateFactorSenzB 0.97 |
mdraganic | 5:7e28f4b64a55 | 11 | #define calibrateFactorSenzC 1.06 |
mdraganic | 5:7e28f4b64a55 | 12 | #define calibrateFactorSenzD 0.9 |
mdraganic | 3:bebfc64cefe4 | 13 | |
mdraganic | 7:ac67f44120a5 | 14 | #define lcdOutputEnable true |
cvitas | 11:f243d8ed556d | 15 | #define serialOutputEnable true |
cvitas | 11:f243d8ed556d | 16 | #define serialOutputBaudrate 9600 |
mdraganic | 4:03b68322905f | 17 | |
mdraganic | 3:bebfc64cefe4 | 18 | Serial pc(USBTX, USBRX); |
mdraganic | 9:764eb7341754 | 19 | TextLCD lcd(p13, p14, p5, p6, p7, p8); // rs, e, d4-d7 |
mdraganic | 7:ac67f44120a5 | 20 | |
cvitas | 11:f243d8ed556d | 21 | Ticker Prikaz; |
cvitas | 11:f243d8ed556d | 22 | |
mdraganic | 7:ac67f44120a5 | 23 | DigitalOut ledBoot(LED1); |
mdraganic | 10:0b8a98b1e6b1 | 24 | DigitalOut ledAz(LED3); |
mdraganic | 7:ac67f44120a5 | 25 | DigitalOut ledEl(LED4); |
mdraganic | 0:7447b8021b33 | 26 | |
mdraganic | 10:0b8a98b1e6b1 | 27 | AnalogIn ainSensA(p16); |
mdraganic | 10:0b8a98b1e6b1 | 28 | AnalogIn ainSensB(p17); |
mdraganic | 10:0b8a98b1e6b1 | 29 | AnalogIn ainSensC(p18); |
mdraganic | 10:0b8a98b1e6b1 | 30 | AnalogIn ainSensD(p19); |
mdraganic | 0:7447b8021b33 | 31 | |
cvitas | 11:f243d8ed556d | 32 | PinDetect pb_azimut(p11); |
cvitas | 11:f243d8ed556d | 33 | PinDetect pb_elevacija(p12); |
cvitas | 11:f243d8ed556d | 34 | |
mdraganic | 2:0bf41ad96558 | 35 | float valAzimut = 0; |
mdraganic | 2:0bf41ad96558 | 36 | float valElevacija = 0; |
mdraganic | 2:0bf41ad96558 | 37 | |
cvitas | 11:f243d8ed556d | 38 | int volatile count_azimut=0; |
cvitas | 11:f243d8ed556d | 39 | int volatile count_elevacija=0; |
cvitas | 11:f243d8ed556d | 40 | |
cvitas | 11:f243d8ed556d | 41 | |
mdraganic | 2:0bf41ad96558 | 42 | float SensA, SensB, SensC, SensD; |
cvitas | 11:f243d8ed556d | 43 | |
cvitas | 11:f243d8ed556d | 44 | // Callback routine is interrupt activated by a debounced pb hit |
cvitas | 11:f243d8ed556d | 45 | void pb_azimut_hit_callback (void) { |
cvitas | 11:f243d8ed556d | 46 | count_azimut++; |
cvitas | 11:f243d8ed556d | 47 | } |
cvitas | 11:f243d8ed556d | 48 | |
cvitas | 11:f243d8ed556d | 49 | void pb_elevacija_hit_callback (void) { |
cvitas | 11:f243d8ed556d | 50 | count_elevacija++; |
cvitas | 11:f243d8ed556d | 51 | } |
cvitas | 11:f243d8ed556d | 52 | |
cvitas | 11:f243d8ed556d | 53 | char GetKeyInput(void) { |
cvitas | 11:f243d8ed556d | 54 | char c = pc.getc(); // get keyboard data (note numerical ascii range) |
cvitas | 11:f243d8ed556d | 55 | pc.printf("%c",c); // print ascii value to host PC terminal |
cvitas | 11:f243d8ed556d | 56 | return (c&0x0F); // return value as non-ascii (bitmask c with value 0x0f |
cvitas | 11:f243d8ed556d | 57 | } |
cvitas | 11:f243d8ed556d | 58 | |
cvitas | 11:f243d8ed556d | 59 | void od_prikaza(){ |
cvitas | 11:f243d8ed556d | 60 | lcd.cls(); |
cvitas | 11:f243d8ed556d | 61 | lcd.printf(" %d %d ", count_azimut , count_elevacija); |
cvitas | 11:f243d8ed556d | 62 | } |
cvitas | 11:f243d8ed556d | 63 | |
mdraganic | 2:0bf41ad96558 | 64 | void readValuesForAveraging() { |
mdraganic | 2:0bf41ad96558 | 65 | |
mdraganic | 2:0bf41ad96558 | 66 | SensA = 0; |
mdraganic | 2:0bf41ad96558 | 67 | SensB = 0; |
mdraganic | 2:0bf41ad96558 | 68 | SensC = 0; |
mdraganic | 2:0bf41ad96558 | 69 | SensD = 0; |
mdraganic | 2:0bf41ad96558 | 70 | |
mdraganic | 3:bebfc64cefe4 | 71 | for (int i = 0; i < 20; i++) { |
mdraganic | 2:0bf41ad96558 | 72 | SensA += ainSensA; |
mdraganic | 2:0bf41ad96558 | 73 | SensB += ainSensB; |
mdraganic | 2:0bf41ad96558 | 74 | SensC += ainSensC; |
mdraganic | 2:0bf41ad96558 | 75 | SensD += ainSensD; |
mdraganic | 2:0bf41ad96558 | 76 | } |
mdraganic | 3:bebfc64cefe4 | 77 | SensA /= 20; |
mdraganic | 3:bebfc64cefe4 | 78 | SensB /= 20; |
mdraganic | 3:bebfc64cefe4 | 79 | SensC /= 20; |
mdraganic | 3:bebfc64cefe4 | 80 | SensD /= 20; |
mdraganic | 3:bebfc64cefe4 | 81 | |
mdraganic | 3:bebfc64cefe4 | 82 | SensA *= calibrateFactorSenzA; |
mdraganic | 3:bebfc64cefe4 | 83 | SensB *= calibrateFactorSenzB; |
mdraganic | 3:bebfc64cefe4 | 84 | SensC *= calibrateFactorSenzC; |
mdraganic | 3:bebfc64cefe4 | 85 | SensD *= calibrateFactorSenzD; |
mdraganic | 2:0bf41ad96558 | 86 | |
mdraganic | 2:0bf41ad96558 | 87 | valAzimut = (SensA + SensB) - (SensC + SensD); |
mdraganic | 2:0bf41ad96558 | 88 | valElevacija = (SensB + SensC) - (SensA + SensD); |
mdraganic | 3:bebfc64cefe4 | 89 | |
mdraganic | 4:03b68322905f | 90 | if(serialOutputEnable) { |
mdraganic | 4:03b68322905f | 91 | |
cvitas | 11:f243d8ed556d | 92 | pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n\r", |
cvitas | 11:f243d8ed556d | 93 | valAzimut, valElevacija, SensA, SensB, SensC, SensD); |
mdraganic | 4:03b68322905f | 94 | |
cvitas | 11:f243d8ed556d | 95 | // pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija); |
mdraganic | 4:03b68322905f | 96 | } |
mdraganic | 7:ac67f44120a5 | 97 | |
mdraganic | 7:ac67f44120a5 | 98 | if(lcdOutputEnable) { |
mdraganic | 7:ac67f44120a5 | 99 | lcd.cls(); |
mdraganic | 8:4044ae40bbc1 | 100 | lcd.printf("a:%5.2f \n", valAzimut); |
mdraganic | 8:4044ae40bbc1 | 101 | lcd.printf("e:%5.2f \n", valElevacija); |
mdraganic | 7:ac67f44120a5 | 102 | } |
mdraganic | 7:ac67f44120a5 | 103 | |
mdraganic | 2:0bf41ad96558 | 104 | } |
mdraganic | 0:7447b8021b33 | 105 | |
cvitas | 11:f243d8ed556d | 106 | char data1, data2; // variable declarations |
mdraganic | 10:0b8a98b1e6b1 | 107 | int main() { |
mdraganic | 0:7447b8021b33 | 108 | |
mdraganic | 7:ac67f44120a5 | 109 | ledBoot = 1; |
cvitas | 11:f243d8ed556d | 110 | Prikaz.attach(&od_prikaza, 0.1); |
cvitas | 11:f243d8ed556d | 111 | |
cvitas | 11:f243d8ed556d | 112 | // Setup Interrupt callback function for a pb hit |
cvitas | 11:f243d8ed556d | 113 | pb_elevacija.attach_deasserted(&pb_elevacija_hit_callback); |
cvitas | 11:f243d8ed556d | 114 | pb_azimut.attach_deasserted(&pb_azimut_hit_callback); |
cvitas | 11:f243d8ed556d | 115 | // Start sampling pb input using interrupts |
cvitas | 11:f243d8ed556d | 116 | pb_elevacija.setSampleFrequency(); |
cvitas | 11:f243d8ed556d | 117 | pb_azimut.setSampleFrequency(); |
mdraganic | 4:03b68322905f | 118 | pc.baud(serialOutputBaudrate); |
cvitas | 11:f243d8ed556d | 119 | |
mdraganic | 10:0b8a98b1e6b1 | 120 | Motor *motorAz = new Motor(p28, p27, p26); // relej, relej, pwm |
mdraganic | 10:0b8a98b1e6b1 | 121 | Motor *motorEl = new Motor(p24, p23, p25); |
mdraganic | 9:764eb7341754 | 122 | if(lcdOutputEnable) { |
mdraganic | 9:764eb7341754 | 123 | lcd.cls(); |
mdraganic | 9:764eb7341754 | 124 | lcd.printf("Suncokre\n"); |
mdraganic | 9:764eb7341754 | 125 | lcd.printf("t TVZ \n"); |
mdraganic | 9:764eb7341754 | 126 | } |
mdraganic | 9:764eb7341754 | 127 | wait_ms(5000); |
mdraganic | 7:ac67f44120a5 | 128 | ledBoot = 0; |
mdraganic | 0:7447b8021b33 | 129 | |
mdraganic | 0:7447b8021b33 | 130 | while(1) { |
cvitas | 11:f243d8ed556d | 131 | |
cvitas | 11:f243d8ed556d | 132 | data2 = GetKeyInput(); // call function to get 1st key press |
cvitas | 11:f243d8ed556d | 133 | switch (data2) { |
cvitas | 11:f243d8ed556d | 134 | case 0: |
cvitas | 11:f243d8ed556d | 135 | (*motorAz).stop(); |
cvitas | 11:f243d8ed556d | 136 | (*motorEl).stop(); |
cvitas | 11:f243d8ed556d | 137 | break; |
cvitas | 11:f243d8ed556d | 138 | case 1: |
cvitas | 11:f243d8ed556d | 139 | (*motorAz).movePositive(); |
cvitas | 11:f243d8ed556d | 140 | break; |
cvitas | 11:f243d8ed556d | 141 | case 2: |
cvitas | 11:f243d8ed556d | 142 | (*motorAz).moveNegative(); |
cvitas | 11:f243d8ed556d | 143 | break; |
cvitas | 11:f243d8ed556d | 144 | case 3: |
cvitas | 11:f243d8ed556d | 145 | (*motorEl).movePositive(); |
cvitas | 11:f243d8ed556d | 146 | break; |
cvitas | 11:f243d8ed556d | 147 | case 4: |
cvitas | 11:f243d8ed556d | 148 | (*motorEl).moveNegative(); |
cvitas | 11:f243d8ed556d | 149 | break; |
cvitas | 11:f243d8ed556d | 150 | default: |
cvitas | 11:f243d8ed556d | 151 | (*motorAz).stop(); |
cvitas | 11:f243d8ed556d | 152 | (*motorEl).stop(); |
cvitas | 11:f243d8ed556d | 153 | break; |
cvitas | 11:f243d8ed556d | 154 | } |
cvitas | 11:f243d8ed556d | 155 | |
cvitas | 11:f243d8ed556d | 156 | /* |
mdraganic | 10:0b8a98b1e6b1 | 157 | // ---- citanje senzora ------------ |
mdraganic | 10:0b8a98b1e6b1 | 158 | |
mdraganic | 2:0bf41ad96558 | 159 | readValuesForAveraging(); |
mdraganic | 0:7447b8021b33 | 160 | |
mdraganic | 3:bebfc64cefe4 | 161 | // ----- azimut ----- |
mdraganic | 3:bebfc64cefe4 | 162 | |
mdraganic | 4:03b68322905f | 163 | if(abs(valAzimut) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 164 | if(valAzimut > 0) |
mdraganic | 3:bebfc64cefe4 | 165 | (*motorAz).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 166 | else |
mdraganic | 3:bebfc64cefe4 | 167 | (*motorAz).moveNegative(); |
mdraganic | 7:ac67f44120a5 | 168 | ledAz = 1; |
mdraganic | 3:bebfc64cefe4 | 169 | } |
mdraganic | 3:bebfc64cefe4 | 170 | |
mdraganic | 4:03b68322905f | 171 | if(abs(valAzimut) < sensorStopTreshold) { |
mdraganic | 3:bebfc64cefe4 | 172 | (*motorAz).stop(); |
mdraganic | 7:ac67f44120a5 | 173 | ledAz = 0; |
mdraganic | 2:0bf41ad96558 | 174 | } |
mdraganic | 3:bebfc64cefe4 | 175 | |
mdraganic | 3:bebfc64cefe4 | 176 | // ----- elevacija ----- |
mdraganic | 3:bebfc64cefe4 | 177 | |
mdraganic | 4:03b68322905f | 178 | if(abs(valElevacija) > sensorStartTreshold) { |
mdraganic | 3:bebfc64cefe4 | 179 | if(valElevacija > 0) |
mdraganic | 3:bebfc64cefe4 | 180 | (*motorEl).movePositive(); |
mdraganic | 3:bebfc64cefe4 | 181 | else |
mdraganic | 3:bebfc64cefe4 | 182 | (*motorEl).moveNegative(); |
mdraganic | 7:ac67f44120a5 | 183 | ledEl = 1; |
mdraganic | 3:bebfc64cefe4 | 184 | } |
mdraganic | 3:bebfc64cefe4 | 185 | |
mdraganic | 4:03b68322905f | 186 | if(abs(valElevacija) < sensorStopTreshold) { |
mdraganic | 4:03b68322905f | 187 | (*motorEl).stop(); |
mdraganic | 7:ac67f44120a5 | 188 | ledEl = 0; |
mdraganic | 3:bebfc64cefe4 | 189 | } |
cvitas | 11:f243d8ed556d | 190 | */ |
mdraganic | 4:03b68322905f | 191 | // ----- pauza ------ |
mdraganic | 4:03b68322905f | 192 | |
cvitas | 11:f243d8ed556d | 193 | |
cvitas | 11:f243d8ed556d | 194 | //wait_ms(200); |
cvitas | 11:f243d8ed556d | 195 | |
mdraganic | 0:7447b8021b33 | 196 | } |
mdraganic | 0:7447b8021b33 | 197 | } |
mdraganic | 2:0bf41ad96558 | 198 | |
mdraganic | 2:0bf41ad96558 | 199 |