Ljubivoj Cvitas / Mbed 2 deprecated SunflowerMach1_Danchi

v1

Dependencies:   PinDetect TextLCD mbed

Fork of SunflowerMach1 by Milan Draganic

main.cpp@7:ac67f44120a5, 2013-11-11 (annotated)

Committer:
mdraganic
Date:
Mon Nov 11 13:14:10 2013 +0000
Revision:
7:ac67f44120a5
Parent:
6:902bec57d9ae
Child:
8:4044ae40bbc1
dodan LCD display samo za az.

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 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 7:ac67f44120a5 69 lcd.printf("a:%6.3f \n", valAzimut);
mdraganic 7:ac67f44120a5 70 }
mdraganic 7:ac67f44120a5 71
mdraganic 2:0bf41ad96558 72 }
mdraganic 0:7447b8021b33 73
mdraganic 0:7447b8021b33 74 int main() {
mdraganic 0:7447b8021b33 75
mdraganic 7:ac67f44120a5 76 ledBoot = 1;
mdraganic 4:03b68322905f 77 pc.baud(serialOutputBaudrate);
mdraganic 3:bebfc64cefe4 78 Motor *motorAz = new Motor(p25, p26, p24);
mdraganic 3:bebfc64cefe4 79 Motor *motorEl = new Motor(p22, p21, p23);
mdraganic 7:ac67f44120a5 80 wait_ms(500);
mdraganic 7:ac67f44120a5 81 ledBoot = 0;
mdraganic 0:7447b8021b33 82
mdraganic 0:7447b8021b33 83 while(1) {
mdraganic 7:ac67f44120a5 84
mdraganic 2:0bf41ad96558 85 readValuesForAveraging();
mdraganic 0:7447b8021b33 86
mdraganic 3:bebfc64cefe4 87 // ----- azimut -----
mdraganic 3:bebfc64cefe4 88
mdraganic 4:03b68322905f 89 if(abs(valAzimut) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 90 if(valAzimut > 0)
mdraganic 3:bebfc64cefe4 91 (*motorAz).movePositive();
mdraganic 3:bebfc64cefe4 92 else
mdraganic 3:bebfc64cefe4 93 (*motorAz).moveNegative();
mdraganic 7:ac67f44120a5 94 ledAz = 1;
mdraganic 3:bebfc64cefe4 95 }
mdraganic 3:bebfc64cefe4 96
mdraganic 4:03b68322905f 97 if(abs(valAzimut) < sensorStopTreshold) {
mdraganic 3:bebfc64cefe4 98 (*motorAz).stop();
mdraganic 7:ac67f44120a5 99 ledAz = 0;
mdraganic 2:0bf41ad96558 100 }
mdraganic 3:bebfc64cefe4 101
mdraganic 3:bebfc64cefe4 102 // ----- elevacija -----
mdraganic 3:bebfc64cefe4 103
mdraganic 4:03b68322905f 104 if(abs(valElevacija) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 105 if(valElevacija > 0)
mdraganic 3:bebfc64cefe4 106 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 107 else
mdraganic 3:bebfc64cefe4 108 (*motorEl).moveNegative();
mdraganic 7:ac67f44120a5 109 ledEl = 1;
mdraganic 3:bebfc64cefe4 110 }
mdraganic 3:bebfc64cefe4 111
mdraganic 4:03b68322905f 112 if(abs(valElevacija) < sensorStopTreshold) {
mdraganic 4:03b68322905f 113 (*motorEl).stop();
mdraganic 7:ac67f44120a5 114 ledEl = 0;
mdraganic 3:bebfc64cefe4 115 }
mdraganic 3:bebfc64cefe4 116
mdraganic 4:03b68322905f 117 // ----- pauza ------
mdraganic 4:03b68322905f 118
mdraganic 7:ac67f44120a5 119 wait_ms(200);
mdraganic 0:7447b8021b33 120 }
mdraganic 0:7447b8021b33 121 }
mdraganic 2:0bf41ad96558 122
mdraganic 2:0bf41ad96558 123