Ljubivoj Cvitas / Mbed 2 deprecated SunflowerMach1_Danchi

v1

Dependencies:   PinDetect TextLCD mbed

Fork of SunflowerMach1 by Milan Draganic

main.cpp@9:764eb7341754, 2013-11-11 (annotated)

Committer:
mdraganic
Date:
Mon Nov 11 14:04:12 2013 +0000
Revision:
9:764eb7341754
Parent:
8:4044ae40bbc1
Child:
10:0b8a98b1e6b1
welcome message na LCD

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 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 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 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 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 7:ac67f44120a5 90
mdraganic 2:0bf41ad96558 91 readValuesForAveraging();
mdraganic 0:7447b8021b33 92
mdraganic 3:bebfc64cefe4 93 // ----- azimut -----
mdraganic 3:bebfc64cefe4 94
mdraganic 4:03b68322905f 95 if(abs(valAzimut) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 96 if(valAzimut > 0)
mdraganic 3:bebfc64cefe4 97 (*motorAz).movePositive();
mdraganic 3:bebfc64cefe4 98 else
mdraganic 3:bebfc64cefe4 99 (*motorAz).moveNegative();
mdraganic 7:ac67f44120a5 100 ledAz = 1;
mdraganic 3:bebfc64cefe4 101 }
mdraganic 3:bebfc64cefe4 102
mdraganic 4:03b68322905f 103 if(abs(valAzimut) < sensorStopTreshold) {
mdraganic 3:bebfc64cefe4 104 (*motorAz).stop();
mdraganic 7:ac67f44120a5 105 ledAz = 0;
mdraganic 2:0bf41ad96558 106 }
mdraganic 3:bebfc64cefe4 107
mdraganic 3:bebfc64cefe4 108 // ----- elevacija -----
mdraganic 3:bebfc64cefe4 109
mdraganic 4:03b68322905f 110 if(abs(valElevacija) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 111 if(valElevacija > 0)
mdraganic 3:bebfc64cefe4 112 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 113 else
mdraganic 3:bebfc64cefe4 114 (*motorEl).moveNegative();
mdraganic 7:ac67f44120a5 115 ledEl = 1;
mdraganic 3:bebfc64cefe4 116 }
mdraganic 3:bebfc64cefe4 117
mdraganic 4:03b68322905f 118 if(abs(valElevacija) < sensorStopTreshold) {
mdraganic 4:03b68322905f 119 (*motorEl).stop();
mdraganic 7:ac67f44120a5 120 ledEl = 0;
mdraganic 3:bebfc64cefe4 121 }
mdraganic 3:bebfc64cefe4 122
mdraganic 4:03b68322905f 123 // ----- pauza ------
mdraganic 4:03b68322905f 124
mdraganic 7:ac67f44120a5 125 wait_ms(200);
mdraganic 0:7447b8021b33 126 }
mdraganic 0:7447b8021b33 127 }
mdraganic 2:0bf41ad96558 128
mdraganic 2:0bf41ad96558 129