Ljubivoj Cvitas / Mbed 2 deprecated SunflowerMach1

Ovo je kopija vertije od Milana

Dependencies:   TextLCD mbed

Fork of SunflowerMach1 by Milan Draganic

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:cb92c0183ec9
Verzija za kalibraciju platna 1

Who changed what in which revision?

UserRevisionLine numberNew 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