Ljubivoj Cvitas / Mbed 2 deprecated SunflowerMach1_Danchi

v1

Dependencies:   PinDetect TextLCD mbed

Fork of SunflowerMach1 by Milan Draganic

main.cpp@10:0b8a98b1e6b1, 2013-11-11 (annotated)

Committer:
mdraganic
Date:
Mon Nov 11 19:01:15 2013 +0000
Revision:
10:0b8a98b1e6b1
Parent:
9:764eb7341754
Child:
11:f243d8ed556d
done and over :)

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"
mdraganic 3:bebfc64cefe4 4
mdraganic 10:0b8a98b1e6b1 5 #define sensorStartTreshold 0.08
mdraganic 10:0b8a98b1e6b1 6 #define sensorStopTreshold 0.02
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 9:764eb7341754 18 TextLCD lcd(p13, p14, p5, p6, p7, p8); // rs, e, d4-d7
mdraganic 7:ac67f44120a5 19
mdraganic 7:ac67f44120a5 20 DigitalOut ledBoot(LED1);
mdraganic 10:0b8a98b1e6b1 21 DigitalOut ledAz(LED3);
mdraganic 7:ac67f44120a5 22 DigitalOut ledEl(LED4);
mdraganic 0:7447b8021b33 23
mdraganic 10:0b8a98b1e6b1 24 AnalogIn ainSensA(p16);
mdraganic 10:0b8a98b1e6b1 25 AnalogIn ainSensB(p17);
mdraganic 10:0b8a98b1e6b1 26 AnalogIn ainSensC(p18);
mdraganic 10:0b8a98b1e6b1 27 AnalogIn ainSensD(p19);
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 10:0b8a98b1e6b1 75 int main() {
mdraganic 0:7447b8021b33 76
mdraganic 7:ac67f44120a5 77 ledBoot = 1;
mdraganic 4:03b68322905f 78 pc.baud(serialOutputBaudrate);
mdraganic 10:0b8a98b1e6b1 79 Motor *motorAz = new Motor(p28, p27, p26); // relej, relej, pwm
mdraganic 10:0b8a98b1e6b1 80 Motor *motorEl = new Motor(p24, p23, p25);
mdraganic 9:764eb7341754 81 if(lcdOutputEnable) {
mdraganic 9:764eb7341754 82 lcd.cls();
mdraganic 9:764eb7341754 83 lcd.printf("Suncokre\n");
mdraganic 9:764eb7341754 84 lcd.printf("t TVZ \n");
mdraganic 9:764eb7341754 85 }
mdraganic 9:764eb7341754 86 wait_ms(5000);
mdraganic 7:ac67f44120a5 87 ledBoot = 0;
mdraganic 0:7447b8021b33 88
mdraganic 0:7447b8021b33 89 while(1) {
mdraganic 10:0b8a98b1e6b1 90
mdraganic 10:0b8a98b1e6b1 91 // ---- citanje senzora ------------
mdraganic 10:0b8a98b1e6b1 92
mdraganic 2:0bf41ad96558 93 readValuesForAveraging();
mdraganic 0:7447b8021b33 94
mdraganic 3:bebfc64cefe4 95 // ----- azimut -----
mdraganic 3:bebfc64cefe4 96
mdraganic 4:03b68322905f 97 if(abs(valAzimut) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 98 if(valAzimut > 0)
mdraganic 3:bebfc64cefe4 99 (*motorAz).movePositive();
mdraganic 3:bebfc64cefe4 100 else
mdraganic 3:bebfc64cefe4 101 (*motorAz).moveNegative();
mdraganic 7:ac67f44120a5 102 ledAz = 1;
mdraganic 3:bebfc64cefe4 103 }
mdraganic 3:bebfc64cefe4 104
mdraganic 4:03b68322905f 105 if(abs(valAzimut) < sensorStopTreshold) {
mdraganic 3:bebfc64cefe4 106 (*motorAz).stop();
mdraganic 7:ac67f44120a5 107 ledAz = 0;
mdraganic 2:0bf41ad96558 108 }
mdraganic 3:bebfc64cefe4 109
mdraganic 3:bebfc64cefe4 110 // ----- elevacija -----
mdraganic 3:bebfc64cefe4 111
mdraganic 4:03b68322905f 112 if(abs(valElevacija) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 113 if(valElevacija > 0)
mdraganic 3:bebfc64cefe4 114 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 115 else
mdraganic 3:bebfc64cefe4 116 (*motorEl).moveNegative();
mdraganic 7:ac67f44120a5 117 ledEl = 1;
mdraganic 3:bebfc64cefe4 118 }
mdraganic 3:bebfc64cefe4 119
mdraganic 4:03b68322905f 120 if(abs(valElevacija) < sensorStopTreshold) {
mdraganic 4:03b68322905f 121 (*motorEl).stop();
mdraganic 7:ac67f44120a5 122 ledEl = 0;
mdraganic 3:bebfc64cefe4 123 }
mdraganic 3:bebfc64cefe4 124
mdraganic 4:03b68322905f 125 // ----- pauza ------
mdraganic 4:03b68322905f 126
mdraganic 10:0b8a98b1e6b1 127 wait_ms(100);
mdraganic 0:7447b8021b33 128 }
mdraganic 0:7447b8021b33 129 }
mdraganic 2:0bf41ad96558 130
mdraganic 2:0bf41ad96558 131