Ljubivoj Cvitas / Mbed 2 deprecated SunflowerMach1_Danchi

v1

Dependencies:   PinDetect TextLCD mbed

Fork of SunflowerMach1 by Milan Draganic

main.cpp@4:03b68322905f, 2013-11-09 (annotated)

Committer:
mdraganic
Date:
Sat Nov 09 18:35:13 2013 +0000
Revision:
4:03b68322905f
Parent:
3:bebfc64cefe4
Child:
5:7e28f4b64a55
popravljena logika upravljanja motorima

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 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 2:0bf41ad96558 70 readValuesForAveraging();
mdraganic 0:7447b8021b33 71
mdraganic 3:bebfc64cefe4 72 // ----- azimut -----
mdraganic 3:bebfc64cefe4 73
mdraganic 4:03b68322905f 74 if(abs(valAzimut) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 75 if(valAzimut > 0)
mdraganic 3:bebfc64cefe4 76 (*motorAz).movePositive();
mdraganic 3:bebfc64cefe4 77 else
mdraganic 3:bebfc64cefe4 78 (*motorAz).moveNegative();
mdraganic 3:bebfc64cefe4 79 }
mdraganic 3:bebfc64cefe4 80
mdraganic 4:03b68322905f 81 if(abs(valAzimut) < sensorStopTreshold) {
mdraganic 3:bebfc64cefe4 82 (*motorAz).stop();
mdraganic 2:0bf41ad96558 83 }
mdraganic 3:bebfc64cefe4 84
mdraganic 3:bebfc64cefe4 85 // ----- elevacija -----
mdraganic 3:bebfc64cefe4 86
mdraganic 4:03b68322905f 87 if(abs(valElevacija) > sensorStartTreshold) {
mdraganic 3:bebfc64cefe4 88 if(valElevacija > 0)
mdraganic 3:bebfc64cefe4 89 (*motorEl).movePositive();
mdraganic 3:bebfc64cefe4 90 else
mdraganic 3:bebfc64cefe4 91 (*motorEl).moveNegative();
mdraganic 3:bebfc64cefe4 92 }
mdraganic 3:bebfc64cefe4 93
mdraganic 4:03b68322905f 94 if(abs(valElevacija) < sensorStopTreshold) {
mdraganic 4:03b68322905f 95 (*motorEl).stop();
mdraganic 3:bebfc64cefe4 96 }
mdraganic 3:bebfc64cefe4 97
mdraganic 4:03b68322905f 98 // ----- pauza ------
mdraganic 4:03b68322905f 99
mdraganic 4:03b68322905f 100 wait_ms(100);
mdraganic 0:7447b8021b33 101 }
mdraganic 0:7447b8021b33 102 }
mdraganic 2:0bf41ad96558 103
mdraganic 2:0bf41ad96558 104