motor i senzor
Dependencies: TextLCD mbed PinDetect
Diff: main.cpp
- Revision:
- 3:bebfc64cefe4
- Parent:
- 2:0bf41ad96558
- Child:
- 4:03b68322905f
diff -r 0bf41ad96558 -r bebfc64cefe4 main.cpp --- a/main.cpp Sat Nov 09 07:04:13 2013 +0000 +++ b/main.cpp Sat Nov 09 13:44:18 2013 +0000 @@ -1,6 +1,15 @@ #include "mbed.h" #include "Motor.h" -#include "MotorPwm.h" + +#define sensorStartTreshold 0.07 +#define sensorStopTreshold 0.02 + +#define calibrateFactorSenzA 1 +#define calibrateFactorSenzB 0.97 +#define calibrateFactorSenzC 1.06 +#define calibrateFactorSenzD 0.9 + +Serial pc(USBTX, USBRX); AnalogIn ainSensA(p17); AnalogIn ainSensB(p18); @@ -18,45 +27,123 @@ SensC = 0; SensD = 0; - for (int i = 0; i <= 9; i++) { + for (int i = 0; i < 20; i++) { SensA += ainSensA; SensB += ainSensB; SensC += ainSensC; SensD += ainSensD; } - SensA /= 10; - SensB /= 10; - SensC /= 10; - SensD /= 10; + SensA /= 20; + SensB /= 20; + SensC /= 20; + SensD /= 20; + + SensA *= calibrateFactorSenzA; + SensB *= calibrateFactorSenzB; + SensC *= calibrateFactorSenzC; + SensD *= calibrateFactorSenzD; valAzimut = (SensA + SensB) - (SensC + SensD); valElevacija = (SensB + SensC) - (SensA + SensD); + +// pc.printf("az:%6.3f el:%6.3f :: A:%.3f B:%.3f C:%.3f D:%.3f \n", +// valAzimut, valElevacija, SensA, SensB, SensC, SensD); + pc.printf("az:%6.3f el:%6.3f \n", valAzimut, valElevacija); } int main() { - Motor *motorEl = new MotorPwm(p25, p26); - Motor *motorAz = new MotorPwm(p23, p24); + Motor *motorAz = new Motor(p25, p26, p24); + Motor *motorEl = new Motor(p22, p21, p23); while(1) { readValuesForAveraging(); - if (valAzimut > 0.2){ // positive azimuth deviation - (*motorAz).movePositive(); + bool someWrokDone = false; + +// ----- azimut ----- + + if(!(*motorAz).isMoving() && (abs(valAzimut) > sensorStartTreshold)) { + if(valAzimut > 0) + (*motorAz).movePositive(); + else + (*motorAz).moveNegative(); + someWrokDone = true; + } + + if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) { + (*motorAz).stop(); + someWrokDone = true; } - else if (valAzimut < -0.2) { // negative azimuth deviation - (*motorAz).moveNegative(); + +// ----- elevacija ----- + + if(!(*motorEl).isMoving() && (abs(valElevacija) > sensorStartTreshold)) { + if(valElevacija > 0) + (*motorEl).movePositive(); + else + (*motorEl).moveNegative(); + someWrokDone = true; } - (*motorAz).stop(); + + if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) { + (*motorEl).stop(); + someWrokDone = true; + } + +// ---- ništa nije dirano ---- + + if(!someWrokDone) + wait_ms(500); + + + - if (valElevacija > 0.2){ // positive azimuth deviation - (*motorEl).movePositive(); + /* + if(valAzimut > sensorStartTreshold) { + if(!(*motorAz).isMoving()) + (*motorAz).movePositive(); + continue; + } + + if(valAzimut < (-1 * sensorStartTreshold)) { + if(!(*motorAz).isMoving()) + (*motorAz).moveNegative(); + continue; + } + + if((*motorAz).isMoving() && (abs(valAzimut) < sensorStopTreshold)) { + (*motorAz).stop(); + } + + if(valElevacija > sensorStartTreshold) { + if(!(*motorEl).isMoving()) + (*motorEl).movePositive(); + continue; } - else if (valElevacija < -0.2) { // negative azimuth deviation + + if(valElevacija < (-1 * sensorStartTreshold)) { + if(!(*motorEl).isMoving()) + (*motorEl).moveNegative(); + continue; + } + + if((*motorEl).isMoving() && (abs(valElevacija) < sensorStopTreshold)) { + (*motorEl).stop(); + } + */ + + /* + if (valElevacija > sensorTreshold){ // positive azimuth deviation + (*motorEl).movePositive(); + pc.printf("elevacija pozitiv \n"); + } + else if (valElevacija < (-1 * sensorTreshold)) { // negative azimuth deviation (*motorEl).moveNegative(); + pc.printf("elevacija negativ \n"); } - (*motorEl).stop(); + */ } }