Milan Draganic / Mbed 2 deprecated SunflowerMach1

motor i senzor

Dependencies:   TextLCD mbed PinDetect

main.cpp@8:4044ae40bbc1, 2013-11-11 (annotated)

Committer:
mdraganic
Date:
Mon Nov 11 13:26:06 2013 +0000
Revision:
8:4044ae40bbc1
Parent:
7:ac67f44120a5
Child:
9:764eb7341754
LCD gotov

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