Milan Draganic / Mbed 2 deprecated SunflowerMach1

motor i senzor

Dependencies:   TextLCD mbed PinDetect

main.cpp@6:902bec57d9ae, 2013-11-11 (annotated)

Committer:
mdraganic
Date:
Mon Nov 11 11:45:55 2013 +0000
Revision:
6:902bec57d9ae
Parent:
5:7e28f4b64a55
Child:
7:ac67f44120a5
gotov driver i tracker

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 6:902bec57d9ae 4 #define sensorStartTreshold 0.05
mdraganic 6:902bec57d9ae 5 #define sensorStopTreshold 0.01
mdraganic 3:bebfc64cefe4 6
mdraganic 5:7e28f4b64a55 7 #define calibrateFactorSenzA 1
mdraganic 5:7e28f4b64a55 8 #define calibrateFactorSenzB 0.97
mdraganic 5:7e28f4b64a55 9 #define calibrateFactorSenzC 1.06
mdraganic 5:7e28f4b64a55 10 #define calibrateFactorSenzD 0.9
mdraganic 3:bebfc64cefe4 11
mdraganic 4:03b68322905f 12 #define serialOutputEnable true
mdraganic 4:03b68322905f 13 #define serialOutputBaudrate 115200
mdraganic 4:03b68322905f 14
mdraganic 3:bebfc64cefe4 15 Serial pc(USBTX, USBRX);
mdraganic 0:7447b8021b33 16
mdraganic 0:7447b8021b33 17 AnalogIn ainSensA(p17);
mdraganic 0:7447b8021b33 18 AnalogIn ainSensB(p18);
mdraganic 0:7447b8021b33 19 AnalogIn ainSensC(p19);
mdraganic 0:7447b8021b33 20 AnalogIn ainSensD(p20);
mdraganic 0:7447b8021b33 21
mdraganic 2:0bf41ad96558 22 float valAzimut = 0;
mdraganic 2:0bf41ad96558 23 float valElevacija = 0;
mdraganic 2:0bf41ad96558 24
mdraganic 2:0bf41ad96558 25 float SensA, SensB, SensC, SensD;
mdraganic 2:0bf41ad96558 26 void readValuesForAveraging() {
mdraganic 2:0bf41ad96558 27
mdraganic 2:0bf41ad96558 28 SensA = 0;
mdraganic 2:0bf41ad96558 29 SensB = 0;
mdraganic 2:0bf41ad96558 30 SensC = 0;
mdraganic 2:0bf41ad96558 31 SensD = 0;
mdraganic 2:0bf41ad96558 32
mdraganic 3:bebfc64cefe4 33 for (int i = 0; i < 20; i++) {
mdraganic 2:0bf41ad96558 34 SensA += ainSensA;
mdraganic 2:0bf41ad96558 35 SensB += ainSensB;
mdraganic 2:0bf41ad96558 36 SensC += ainSensC;
mdraganic 2:0bf41ad96558 37 SensD += ainSensD;
mdraganic 2:0bf41ad96558 38 }
mdraganic 3:bebfc64cefe4 39 SensA /= 20;
mdraganic 3:bebfc64cefe4 40 SensB /= 20;
mdraganic 3:bebfc64cefe4 41 SensC /= 20;
mdraganic 3:bebfc64cefe4 42 SensD /= 20;
mdraganic 3:bebfc64cefe4 43
mdraganic 3:bebfc64cefe4 44 SensA *= calibrateFactorSenzA;
mdraganic 3:bebfc64cefe4 45 SensB *= calibrateFactorSenzB;
mdraganic 3:bebfc64cefe4 46 SensC *= calibrateFactorSenzC;
mdraganic 3:bebfc64cefe4 47 SensD *= calibrateFactorSenzD;
mdraganic 2:0bf41ad96558 48
mdraganic 2:0bf41ad96558 49 valAzimut = (SensA + SensB) - (SensC + SensD);
mdraganic 2:0bf41ad96558 50 valElevacija = (SensB + SensC) - (SensA + SensD);
mdraganic 3:bebfc64cefe4 51
mdraganic 4:03b68322905f 52 if(serialOutputEnable) {
mdraganic 4:03b68322905f 53
mdraganic 3:bebfc64cefe4 54 // pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n",
mdraganic 3:bebfc64cefe4 55 // valAzimut, valElevacija, SensA, SensB, SensC, SensD);
mdraganic 4:03b68322905f 56
mdraganic 4:03b68322905f 57 pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija);
mdraganic 4:03b68322905f 58 }
mdraganic 2:0bf41ad96558 59 }
mdraganic 0:7447b8021b33 60
mdraganic 0:7447b8021b33 61 int main() {
mdraganic 0:7447b8021b33 62
mdraganic 4:03b68322905f 63 pc.baud(serialOutputBaudrate);
mdraganic 4:03b68322905f 64
mdraganic 3:bebfc64cefe4 65 Motor *motorAz = new Motor(p25, p26, p24);
mdraganic 3:bebfc64cefe4 66 Motor *motorEl = new Motor(p22, p21, p23);
mdraganic 0:7447b8021b33 67
mdraganic 0:7447b8021b33 68 while(1) {
mdraganic 0:7447b8021b33 69
mdraganic 6:902bec57d9ae 70 (*motorEl).movePositive();
mdraganic 6:902bec57d9ae 71 (*motorAz).movePositive();
mdraganic 6:902bec57d9ae 72 wait(5);
mdraganic 6:902bec57d9ae 73 (*motorAz).moveNegative();
mdraganic 6:902bec57d9ae 74 (*motorEl).moveNegative();
mdraganic 6:902bec57d9ae 75 wait(3);
mdraganic 6:902bec57d9ae 76 continue;
mdraganic 6:902bec57d9ae 77
mdraganic 6:902bec57d9ae 78
mdraganic 6:902bec57d9ae 79
mdraganic 2:0bf41ad96558 80 readValuesForAveraging();
mdraganic 0:7447b8021b33 81
mdraganic 3:bebfc64cefe4 82 // ----- azimut -----
mdraganic 3:bebfc64cefe4 83
mdraganic 4:03b68322905f 84 if(abs(valAzimut) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 85 if(valAzimut > 0)
mdraganic 3:bebfc64cefe4 86 (*motorAz).movePositive();
mdraganic 3:bebfc64cefe4 87 else
mdraganic 3:bebfc64cefe4 88 (*motorAz).moveNegative();
mdraganic 3:bebfc64cefe4 89 }
mdraganic 3:bebfc64cefe4 90
mdraganic 4:03b68322905f 91 if(abs(valAzimut) < sensorStopTreshold) {
mdraganic 3:bebfc64cefe4 92 (*motorAz).stop();
mdraganic 2:0bf41ad96558 93 }
mdraganic 3:bebfc64cefe4 94
mdraganic 3:bebfc64cefe4 95 // ----- elevacija -----
mdraganic 3:bebfc64cefe4 96
mdraganic 4:03b68322905f 97 if(abs(valElevacija) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 98 if(valElevacija > 0)
mdraganic 3:bebfc64cefe4 99 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 100 else
mdraganic 3:bebfc64cefe4 101 (*motorEl).moveNegative();
mdraganic 3:bebfc64cefe4 102 }
mdraganic 3:bebfc64cefe4 103
mdraganic 4:03b68322905f 104 if(abs(valElevacija) < sensorStopTreshold) {
mdraganic 4:03b68322905f 105 (*motorEl).stop();
mdraganic 3:bebfc64cefe4 106 }
mdraganic 3:bebfc64cefe4 107
mdraganic 4:03b68322905f 108 // ----- pauza ------
mdraganic 4:03b68322905f 109
mdraganic 4:03b68322905f 110 wait_ms(100);
mdraganic 0:7447b8021b33 111 }
mdraganic 0:7447b8021b33 112 }
mdraganic 2:0bf41ad96558 113
mdraganic 2:0bf41ad96558 114