Ljubivoj Cvitas / Mbed 2 deprecated SunflowerMach1_Danchi

v1

Dependencies:   PinDetect TextLCD mbed

Fork of SunflowerMach1 by Milan Draganic

main.cpp@3:bebfc64cefe4, 2013-11-09 (annotated)

Committer:
mdraganic
Date:
Sat Nov 09 13:44:18 2013 +0000
Revision:
3:bebfc64cefe4
Parent:
2:0bf41ad96558
Child:
4:03b68322905f
sve skupa i zajedno

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 3:bebfc64cefe4 3
mdraganic 3:bebfc64cefe4 4 #define sensorStartTreshold 0.07
mdraganic 3:bebfc64cefe4 5 #define sensorStopTreshold 0.02
mdraganic 3:bebfc64cefe4 6
mdraganic 3:bebfc64cefe4 7 #define calibrateFactorSenzA 1
mdraganic 3:bebfc64cefe4 8 #define calibrateFactorSenzB 0.97
mdraganic 3:bebfc64cefe4 9 #define calibrateFactorSenzC 1.06
mdraganic 3:bebfc64cefe4 10 #define calibrateFactorSenzD 0.9
mdraganic 3:bebfc64cefe4 11
mdraganic 3:bebfc64cefe4 12 Serial pc(USBTX, USBRX);
mdraganic 0:7447b8021b33 13
mdraganic 0:7447b8021b33 14 AnalogIn ainSensA(p17);
mdraganic 0:7447b8021b33 15 AnalogIn ainSensB(p18);
mdraganic 0:7447b8021b33 16 AnalogIn ainSensC(p19);
mdraganic 0:7447b8021b33 17 AnalogIn ainSensD(p20);
mdraganic 0:7447b8021b33 18
mdraganic 2:0bf41ad96558 19 float valAzimut = 0;
mdraganic 2:0bf41ad96558 20 float valElevacija = 0;
mdraganic 2:0bf41ad96558 21
mdraganic 2:0bf41ad96558 22 float SensA, SensB, SensC, SensD;
mdraganic 2:0bf41ad96558 23 void readValuesForAveraging() {
mdraganic 2:0bf41ad96558 24
mdraganic 2:0bf41ad96558 25 SensA = 0;
mdraganic 2:0bf41ad96558 26 SensB = 0;
mdraganic 2:0bf41ad96558 27 SensC = 0;
mdraganic 2:0bf41ad96558 28 SensD = 0;
mdraganic 2:0bf41ad96558 29
mdraganic 3:bebfc64cefe4 30 for (int i = 0; i < 20; i++) {
mdraganic 2:0bf41ad96558 31 SensA += ainSensA;
mdraganic 2:0bf41ad96558 32 SensB += ainSensB;
mdraganic 2:0bf41ad96558 33 SensC += ainSensC;
mdraganic 2:0bf41ad96558 34 SensD += ainSensD;
mdraganic 2:0bf41ad96558 35 }
mdraganic 3:bebfc64cefe4 36 SensA /= 20;
mdraganic 3:bebfc64cefe4 37 SensB /= 20;
mdraganic 3:bebfc64cefe4 38 SensC /= 20;
mdraganic 3:bebfc64cefe4 39 SensD /= 20;
mdraganic 3:bebfc64cefe4 40
mdraganic 3:bebfc64cefe4 41 SensA *= calibrateFactorSenzA;
mdraganic 3:bebfc64cefe4 42 SensB *= calibrateFactorSenzB;
mdraganic 3:bebfc64cefe4 43 SensC *= calibrateFactorSenzC;
mdraganic 3:bebfc64cefe4 44 SensD *= calibrateFactorSenzD;
mdraganic 2:0bf41ad96558 45
mdraganic 2:0bf41ad96558 46 valAzimut = (SensA + SensB) - (SensC + SensD);
mdraganic 2:0bf41ad96558 47 valElevacija = (SensB + SensC) - (SensA + SensD);
mdraganic 3:bebfc64cefe4 48
mdraganic 3:bebfc64cefe4 49 // pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n",
mdraganic 3:bebfc64cefe4 50 // valAzimut, valElevacija, SensA, SensB, SensC, SensD);
mdraganic 3:bebfc64cefe4 51 pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija);
mdraganic 2:0bf41ad96558 52 }
mdraganic 0:7447b8021b33 53
mdraganic 0:7447b8021b33 54 int main() {
mdraganic 0:7447b8021b33 55
mdraganic 3:bebfc64cefe4 56 Motor *motorAz = new Motor(p25, p26, p24);
mdraganic 3:bebfc64cefe4 57 Motor *motorEl = new Motor(p22, p21, p23);
mdraganic 0:7447b8021b33 58
mdraganic 0:7447b8021b33 59 while(1) {
mdraganic 0:7447b8021b33 60
mdraganic 2:0bf41ad96558 61 readValuesForAveraging();
mdraganic 0:7447b8021b33 62
mdraganic 3:bebfc64cefe4 63 bool someWrokDone = false;
mdraganic 3:bebfc64cefe4 64
mdraganic 3:bebfc64cefe4 65 // ----- azimut -----
mdraganic 3:bebfc64cefe4 66
mdraganic 3:bebfc64cefe4 67 if(!(*motorAz).isMoving() && (abs(valAzimut) > sensorStartTreshold)) {
mdraganic 3:bebfc64cefe4 68 if(valAzimut > 0)
mdraganic 3:bebfc64cefe4 69 (*motorAz).movePositive();
mdraganic 3:bebfc64cefe4 70 else
mdraganic 3:bebfc64cefe4 71 (*motorAz).moveNegative();
mdraganic 3:bebfc64cefe4 72 someWrokDone = true;
mdraganic 3:bebfc64cefe4 73 }
mdraganic 3:bebfc64cefe4 74
mdraganic 3:bebfc64cefe4 75 if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) {
mdraganic 3:bebfc64cefe4 76 (*motorAz).stop();
mdraganic 3:bebfc64cefe4 77 someWrokDone = true;
mdraganic 2:0bf41ad96558 78 }
mdraganic 3:bebfc64cefe4 79
mdraganic 3:bebfc64cefe4 80 // ----- elevacija -----
mdraganic 3:bebfc64cefe4 81
mdraganic 3:bebfc64cefe4 82 if(!(*motorEl).isMoving() && (abs(valElevacija) > sensorStartTreshold)) {
mdraganic 3:bebfc64cefe4 83 if(valElevacija > 0)
mdraganic 3:bebfc64cefe4 84 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 85 else
mdraganic 3:bebfc64cefe4 86 (*motorEl).moveNegative();
mdraganic 3:bebfc64cefe4 87 someWrokDone = true;
mdraganic 2:0bf41ad96558 88 }
mdraganic 3:bebfc64cefe4 89
mdraganic 3:bebfc64cefe4 90 if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) {
mdraganic 3:bebfc64cefe4 91 (*motorEl).stop();
mdraganic 3:bebfc64cefe4 92 someWrokDone = true;
mdraganic 3:bebfc64cefe4 93 }
mdraganic 3:bebfc64cefe4 94
mdraganic 3:bebfc64cefe4 95 // ---- ništa nije dirano ----
mdraganic 3:bebfc64cefe4 96
mdraganic 3:bebfc64cefe4 97 if(!someWrokDone)
mdraganic 3:bebfc64cefe4 98 wait_ms(500);
mdraganic 3:bebfc64cefe4 99
mdraganic 3:bebfc64cefe4 100
mdraganic 3:bebfc64cefe4 101
mdraganic 2:0bf41ad96558 102
mdraganic 3:bebfc64cefe4 103 /*
mdraganic 3:bebfc64cefe4 104 if(valAzimut > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 105 if(!(*motorAz).isMoving())
mdraganic 3:bebfc64cefe4 106 (*motorAz).movePositive();
mdraganic 3:bebfc64cefe4 107 continue;
mdraganic 3:bebfc64cefe4 108 }
mdraganic 3:bebfc64cefe4 109
mdraganic 3:bebfc64cefe4 110 if(valAzimut < (-1 * sensorStartTreshold)) {
mdraganic 3:bebfc64cefe4 111 if(!(*motorAz).isMoving())
mdraganic 3:bebfc64cefe4 112 (*motorAz).moveNegative();
mdraganic 3:bebfc64cefe4 113 continue;
mdraganic 3:bebfc64cefe4 114 }
mdraganic 3:bebfc64cefe4 115
mdraganic 3:bebfc64cefe4 116 if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) {
mdraganic 3:bebfc64cefe4 117 (*motorAz).stop();
mdraganic 3:bebfc64cefe4 118 }
mdraganic 3:bebfc64cefe4 119
mdraganic 3:bebfc64cefe4 120 if(valElevacija > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 121 if(!(*motorEl).isMoving())
mdraganic 3:bebfc64cefe4 122 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 123 continue;
mdraganic 2:0bf41ad96558 124 }
mdraganic 3:bebfc64cefe4 125
mdraganic 3:bebfc64cefe4 126 if(valElevacija < (-1 * sensorStartTreshold)) {
mdraganic 3:bebfc64cefe4 127 if(!(*motorEl).isMoving())
mdraganic 3:bebfc64cefe4 128 (*motorEl).moveNegative();
mdraganic 3:bebfc64cefe4 129 continue;
mdraganic 3:bebfc64cefe4 130 }
mdraganic 3:bebfc64cefe4 131
mdraganic 3:bebfc64cefe4 132 if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) {
mdraganic 3:bebfc64cefe4 133 (*motorEl).stop();
mdraganic 3:bebfc64cefe4 134 }
mdraganic 3:bebfc64cefe4 135 */
mdraganic 3:bebfc64cefe4 136
mdraganic 3:bebfc64cefe4 137 /*
mdraganic 3:bebfc64cefe4 138 if (valElevacija > sensorTreshold){ // positive azimuth deviation
mdraganic 3:bebfc64cefe4 139 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 140 pc.printf("elevacija pozitiv \n");
mdraganic 3:bebfc64cefe4 141 }
mdraganic 3:bebfc64cefe4 142 else if (valElevacija < (-1 * sensorTreshold)) { // negative azimuth deviation
mdraganic 2:0bf41ad96558 143 (*motorEl).moveNegative();
mdraganic 3:bebfc64cefe4 144 pc.printf("elevacija negativ \n");
mdraganic 2:0bf41ad96558 145 }
mdraganic 3:bebfc64cefe4 146 */
mdraganic 0:7447b8021b33 147 }
mdraganic 0:7447b8021b33 148 }
mdraganic 2:0bf41ad96558 149
mdraganic 2:0bf41ad96558 150