Commit ebbf5537 authored by Markus Mueller's avatar Markus Mueller

Add shouldRotate state

parent 028e5d26
......@@ -35,6 +35,7 @@ void CRotor::initRotor()
//Initial Values
m_bAzimuthBreakReleased = 0;
m_bAzimuthCurrentlyRotating = 0;
m_bAzimuthShouldRotate = 0;
}
if(ELEV)
{
......@@ -49,6 +50,7 @@ void CRotor::initRotor()
//Initial Values
m_bElevationBreakReleased = 0;
m_bElevationCurrentlyRotating = 0;
m_bElevationShouldRotate = 0;
}
}
......@@ -58,34 +60,36 @@ void CRotor::initRotor()
void CRotor::doRotor()
{
int diff_elev, diff_azim;
readRotSensors();
if(AZIM && false)
if(AZIM && m_bAzimuthShouldRotate)
{
diff_azim = m_uActualAzimuth - m_uSetAzimuth;
if(m_bAzimuthCurrentlyRotating) {
if(abs(m_uActualAzimuth - m_uSetAzimuth) < AZIM_RES)
if(abs(diff_azim) < AZIM_RES)
stopAzimuth();
else {
if(m_uActualAzimuth - m_uSetAzimuth > 0)
if(diff_azim > 0)
rotateCW();
else
rotateCCW();
}
}
else {
if(abs(m_uActualAzimuth - m_uSetAzimuth) > AZIM_SPAN) {
if(abs(diff_azim) > AZIM_SPAN) {
releaseBreakAzimuth();
if(m_uActualAzimuth - m_uSetAzimuth > 0)
if(diff_azim > 0)
rotateCW();
else
rotateCCW();
}
}
}
if(ELEV)
if(ELEV && m_bElevationShouldRotate)
{
int diff = m_uActualElevation - m_uSetElevation;
diff_elev = m_uActualElevation - m_uSetElevation;
if(m_bElevationCurrentlyRotating) {
if(abs(diff) < ELEV_RES) {
if(abs(diff_elev) < ELEV_RES) {
stopElevation();
if(m_uSetElevation >= 90)
{setElevation(20);}
......@@ -93,16 +97,16 @@ void CRotor::doRotor()
{setElevation(130);}
}
else {
if(diff > 0)
if(diff_elev > 0)
rotateUp();
else
rotateDown();
}
}
else {
if(abs(diff) > ELEV_SPAN) {
if(abs(diff_elev) > ELEV_SPAN) {
releaseBreakElevation();
if(diff > 0)
if(diff_elev > 0)
rotateUp();
else
rotateDown();
......@@ -140,6 +144,7 @@ void CRotor::stopAzimuth()
digitalWrite(AZIM_RE_CW, HIGH);
digitalWrite(AZIM_RE_CCW, HIGH);
m_bAzimuthCurrentlyRotating = 0;
m_bAzimuthShouldRotate = 0;
setAzimuth(m_uActualAzimuth);
}
......@@ -148,6 +153,7 @@ void CRotor::stopElevation()
digitalWrite(ELEV_RE_UP, HIGH);
digitalWrite(ELEV_RE_DWN, HIGH);
m_bElevationCurrentlyRotating = 0;
m_bElevationShouldRotate = 0;
setElevation(m_uActualElevation);
}
......@@ -242,6 +248,7 @@ CRotor::rotorError CRotor::setAzimuth(uint16 pAzimuth)
if(pAzimuth >= AZIM_MIN && pAzimuth <= AZIM_MAX)
{
m_uSetAzimuth = pAzimuth;
m_bAzimuthShouldRotate = 1;
return CRotor::ROT_OK;
}
else
......@@ -263,6 +270,7 @@ CRotor::rotorError CRotor::setElevation(uint16 pElevation)
if(pElevation >= ELEV_MIN && pElevation <= ELEV_MAX)
{
m_uSetElevation = pElevation;
m_bElevationShouldRotate = 1;
return CRotor::ROT_OK;
}
else
......
......@@ -51,6 +51,8 @@ private:
byte m_bElevationBreakReleased;
byte m_bAzimuthCurrentlyRotating;
byte m_bElevationCurrentlyRotating;
byte m_bAzimuthShouldRotate;
byte m_bElevationShouldRotate;
};
#endif //ROTOR_H
......@@ -45,7 +45,7 @@ void setup()
//Debug
debugTime = 0;
rotor.setElevation(70);
rotor.setElevation(170);
}
void loop()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment