rotor.cpp 6.79 KB
Newer Older
andz's avatar
andz committed
1
2
3
4
5
6
///
/// @file       rotor.cpp
/// @copyright  Christian Obersteiner, Andreas Müller - CC-BY-SA 4.0
///
/// @brief  Contains all kind of rotor functions
///
chris007's avatar
chris007 committed
7
8
9

#include "rotor.h"

andz's avatar
andz committed
10
11
12
13
///
/// @brief  Constructor
///

Markus's avatar
Markus committed
14
Rotor::Rotor()
chris007's avatar
chris007 committed
15
{
16
    initRotor();
chris007's avatar
chris007 committed
17
18
}

andz's avatar
andz committed
19
20
21
22
///
/// @brief  Initialize all ADC Values and set Arduino Pins to a defined value.
///

Markus's avatar
Markus committed
23
void Rotor::initRotor()
chris007's avatar
chris007 committed
24
{
andz's avatar
andz committed
25
26
27
28
29
30
31
32
33
34
35
    if(AZIM)
    {
        //Pin Settings
        pinMode(AZIM_RE_CW, OUTPUT);
        pinMode(AZIM_RE_CCW, OUTPUT);
        pinMode(AZIM_RE_BREAK, OUTPUT);
        //All Relais Off
        digitalWrite(AZIM_RE_CW, HIGH); 
        digitalWrite(AZIM_RE_CCW, HIGH); 
        digitalWrite(AZIM_RE_BREAK, HIGH);
        //Initial Values
36
        m_bAzimuthBreakReleased = 0;
Markus Mueller's avatar
Markus Mueller committed
37
        m_bAzimuthCurrentlyRotating = 0;
Markus Mueller's avatar
Markus Mueller committed
38
        m_bAzimuthShouldRotate = 0;
andz's avatar
andz committed
39
40
41
42
43
44
45
46
    }
    if(ELEV)
    {
        //Pin Settings
        pinMode(ELEV_RE_UP, OUTPUT);
        pinMode(ELEV_RE_DWN, OUTPUT);
        pinMode(ELEV_RE_BREAK, OUTPUT);
        //All Relais Off
47
48
49
50
51
        digitalWrite(ELEV_RE_UP, HIGH);
        digitalWrite(ELEV_RE_DWN, HIGH);
        digitalWrite(ELEV_RE_BREAK, HIGH);
        //Initial Values
        m_bElevationBreakReleased = 0;
Markus Mueller's avatar
Markus Mueller committed
52
        m_bElevationCurrentlyRotating = 0;
Markus Mueller's avatar
Markus Mueller committed
53
        m_bElevationShouldRotate = 0;
andz's avatar
andz committed
54
    }
chris007's avatar
chris007 committed
55
56
}

andz's avatar
andz committed
57
58
59
60
///
/// @brief  Does all the rotor movement stuff
///

Markus's avatar
Markus committed
61
void Rotor::doRotor()
chris007's avatar
chris007 committed
62
{
Markus Mueller's avatar
Markus Mueller committed
63
    int diff_elev, diff_azim;
andz's avatar
andz committed
64
    readRotSensors();
Markus Mueller's avatar
Markus Mueller committed
65
    if(AZIM && m_bAzimuthShouldRotate)
andz's avatar
andz committed
66
    {
Markus Mueller's avatar
Markus Mueller committed
67
        diff_azim = m_uActualAzimuth - m_uSetAzimuth;
Markus Mueller's avatar
Markus Mueller committed
68
        if(m_bAzimuthCurrentlyRotating) {
Markus Mueller's avatar
Markus Mueller committed
69
            if(abs(diff_azim) < AZIM_RES)
Markus Mueller's avatar
Markus Mueller committed
70
71
                stopAzimuth();
            else {
Markus Mueller's avatar
Markus Mueller committed
72
              if(diff_azim > 0)
Markus Mueller's avatar
Markus Mueller committed
73
74
75
76
77
78
                rotateCW();
              else
                rotateCCW();
            }
        }
        else {
Markus Mueller's avatar
Markus Mueller committed
79
            if(abs(diff_azim) > AZIM_SPAN) {
Markus Mueller's avatar
Markus Mueller committed
80
              releaseBreakAzimuth();
Markus Mueller's avatar
Markus Mueller committed
81
              if(diff_azim > 0)
Markus Mueller's avatar
Markus Mueller committed
82
83
84
85
86
                rotateCW();
              else
                rotateCCW();
            }
        }
andz's avatar
andz committed
87
    }
Markus Mueller's avatar
Markus Mueller committed
88
    if(ELEV && m_bElevationShouldRotate)
andz's avatar
andz committed
89
    {
Markus Mueller's avatar
Markus Mueller committed
90
        diff_elev = m_uActualElevation - m_uSetElevation;
Markus Mueller's avatar
Markus Mueller committed
91
        if(m_bElevationCurrentlyRotating) {
Markus's avatar
Markus committed
92
            if(abs(diff_elev) < ELEV_RES)
Markus Mueller's avatar
Markus Mueller committed
93
94
                stopElevation();
            else {
Markus Mueller's avatar
Markus Mueller committed
95
              if(diff_elev > 0)
Markus Mueller's avatar
Markus Mueller committed
96
                rotateDown();
markus's avatar
markus committed
97
98
              else
                rotateUp();
Markus Mueller's avatar
Markus Mueller committed
99
100
101
            }
        }
        else {
Markus Mueller's avatar
Markus Mueller committed
102
            if(abs(diff_elev) > ELEV_SPAN) {
Markus Mueller's avatar
Markus Mueller committed
103
              releaseBreakElevation();
Markus Mueller's avatar
Markus Mueller committed
104
              if(diff_elev > 0)
Markus Mueller's avatar
Markus Mueller committed
105
                rotateDown();
markus's avatar
markus committed
106
107
              else
                rotateUp();
Markus Mueller's avatar
Markus Mueller committed
108
109
            }
        }
andz's avatar
andz committed
110
    }
Markus's avatar
Markus committed
111

Markus's avatar
Markus committed
112
    if ( !m_bAzimuthCurrentlyRotating && !m_bElevationCurrentlyRotating ) {
Markus's avatar
Markus committed
113
114
115
        tightenBreakAzimuth();
        tightenBreakElevation();
    }
Markus Mueller's avatar
Markus Mueller committed
116
117
}

Markus's avatar
Markus committed
118
void Rotor::releaseBreakAzimuth()
Markus Mueller's avatar
Markus Mueller committed
119
120
121
122
123
{
    m_bAzimuthBreakReleased = 1;
    digitalWrite(AZIM_RE_BREAK, LOW);
}

Markus's avatar
Markus committed
124
void Rotor::releaseBreakElevation()
Markus Mueller's avatar
Markus Mueller committed
125
126
127
128
129
{
    m_bElevationBreakReleased = 1;
    digitalWrite(ELEV_RE_BREAK, LOW);
}

Markus's avatar
Markus committed
130
void Rotor::tightenBreakAzimuth()
Markus Mueller's avatar
Markus Mueller committed
131
132
133
134
135
{
    m_bAzimuthBreakReleased = 0;
    digitalWrite(AZIM_RE_BREAK, HIGH);
}

Markus's avatar
Markus committed
136
void Rotor::tightenBreakElevation()
Markus Mueller's avatar
Markus Mueller committed
137
138
139
140
141
{
    m_bElevationBreakReleased = 0;
    digitalWrite(ELEV_RE_BREAK, HIGH);
}

Markus's avatar
Markus committed
142
void Rotor::stopAzimuth()
Markus Mueller's avatar
Markus Mueller committed
143
144
145
146
{
    digitalWrite(AZIM_RE_CW, HIGH);
    digitalWrite(AZIM_RE_CCW, HIGH);
    m_bAzimuthCurrentlyRotating = 0;
Markus Mueller's avatar
Markus Mueller committed
147
    m_bAzimuthShouldRotate = 0;
Markus Mueller's avatar
Markus Mueller committed
148
149
}

Markus's avatar
Markus committed
150
void Rotor::stopElevation()
Markus Mueller's avatar
Markus Mueller committed
151
152
153
154
{
    digitalWrite(ELEV_RE_UP, HIGH);
    digitalWrite(ELEV_RE_DWN, HIGH);
    m_bElevationCurrentlyRotating = 0;
Markus Mueller's avatar
Markus Mueller committed
155
    m_bElevationShouldRotate = 0;
Markus Mueller's avatar
Markus Mueller committed
156
157
}

Markus's avatar
Markus committed
158
Rotor::rotorError Rotor::rotateCW()
Markus Mueller's avatar
Markus Mueller committed
159
160
{
    if ( m_bAzimuthBreakReleased == 0 )
Markus's avatar
Markus committed
161
        return Rotor::ROT_BREAK_NOT_RELEASED;
Markus Mueller's avatar
Markus Mueller committed
162
163
164
    digitalWrite(AZIM_RE_CW, LOW);
    digitalWrite(AZIM_RE_CCW, HIGH);
    m_bAzimuthCurrentlyRotating = 1;
Markus's avatar
Markus committed
165
    return Rotor::ROT_OK;
Markus Mueller's avatar
Markus Mueller committed
166
}
andz's avatar
andz committed
167

Markus's avatar
Markus committed
168
Rotor::rotorError Rotor::rotateCCW()
Markus Mueller's avatar
Markus Mueller committed
169
170
{
    if ( m_bAzimuthBreakReleased == 0 )
Markus's avatar
Markus committed
171
        return Rotor::ROT_BREAK_NOT_RELEASED;
Markus Mueller's avatar
Markus Mueller committed
172
173
174
    digitalWrite(AZIM_RE_CW, HIGH);
    digitalWrite(AZIM_RE_CCW, LOW);
    m_bAzimuthCurrentlyRotating = 1;
Markus's avatar
Markus committed
175
    return Rotor::ROT_OK;
andz's avatar
andz committed
176
177
}

Markus's avatar
Markus committed
178
Rotor::rotorError Rotor::rotateUp()
Markus Mueller's avatar
Markus Mueller committed
179
180
{
    if ( m_bElevationBreakReleased == 0 )
Markus's avatar
Markus committed
181
        return Rotor::ROT_BREAK_NOT_RELEASED;
Markus Mueller's avatar
Markus Mueller committed
182
183
184
    digitalWrite(ELEV_RE_UP, LOW);
    digitalWrite(ELEV_RE_DWN, HIGH);
    m_bElevationCurrentlyRotating = 1;
Markus's avatar
Markus committed
185
    return Rotor::ROT_OK;
Markus Mueller's avatar
Markus Mueller committed
186
187
}

Markus's avatar
Markus committed
188
Rotor::rotorError Rotor::rotateDown()
Markus Mueller's avatar
Markus Mueller committed
189
190
{
    if ( m_bElevationBreakReleased == 0 )
Markus's avatar
Markus committed
191
        return Rotor::ROT_BREAK_NOT_RELEASED;
Markus Mueller's avatar
Markus Mueller committed
192
193
194
    digitalWrite(ELEV_RE_UP, HIGH);
    digitalWrite(ELEV_RE_DWN, LOW);
    m_bElevationCurrentlyRotating = 1;
Markus's avatar
Markus committed
195
    return Rotor::ROT_OK;
Markus Mueller's avatar
Markus Mueller committed
196
197
198
}


andz's avatar
andz committed
199
200
201
202
///
/// @brief  Serial Debug output Function
///

Markus's avatar
Markus committed
203
void Rotor::debugOut()
andz's avatar
andz committed
204
205
206
207
{
    //debug Out
    readRotSensors();
    Serial.println("-----------------");
Markus Mueller's avatar
Markus Mueller committed
208
209
210
211
    Serial.print("Azim Value: ");
    Serial.println(m_uActualAzimuth);
    Serial.print("Elev Value: ");
    Serial.println(m_uActualElevation);
andz's avatar
andz committed
212
213
}

andz's avatar
andz committed
214
215
216
217
218
219
///
/// @brief Returns the actual Azimuth value in degree
///
/// @return uint16 actual Azimuth value in degree
///

Markus's avatar
Markus committed
220
uint16 Rotor::getActualAzimuth() const
andz's avatar
andz committed
221
222
223
224
{
    return m_uActualAzimuth;
}

andz's avatar
andz committed
225
226
227
228
229
230
///
/// @brief Returns the actual Elevation value in degree
///
/// @return actual Elevation value in degree
///

Markus's avatar
Markus committed
231
uint16 Rotor::getActualElevation() const
andz's avatar
andz committed
232
233
{
    return m_uActualElevation;
chris007's avatar
chris007 committed
234
235
}

andz's avatar
andz committed
236
237
238
239
240
241
242
243
///
/// @brief Sets the actual Azimuth value in degree and checks valid range
///
/// @param pAzimuth Azimuth value to be set
///
/// @return Error Code
///

Markus's avatar
Markus committed
244
Rotor::rotorError Rotor::setAzimuth(uint16 pAzimuth)
chris007's avatar
chris007 committed
245
{
andz's avatar
andz committed
246
247
248
    if(pAzimuth >= AZIM_MIN && pAzimuth <= AZIM_MAX)
    {
        m_uSetAzimuth = pAzimuth;
Markus Mueller's avatar
Markus Mueller committed
249
        m_bAzimuthShouldRotate = 1;
Markus's avatar
Markus committed
250
        return Rotor::ROT_OK;
andz's avatar
andz committed
251
252
253
    }
    else
    {
Markus's avatar
Markus committed
254
        return Rotor::ROT_VAL_OUT_OF_RANGE;
andz's avatar
andz committed
255
    }
chris007's avatar
chris007 committed
256
257
}

andz's avatar
andz committed
258
259
260
261
262
263
264
265
///
/// @brief Sets the actual Elevation value in degree and checks valid range
///
/// @param pElevation Elevation value to be set
///
/// @return Error Code
///

Markus's avatar
Markus committed
266
Rotor::rotorError Rotor::setElevation(uint16 pElevation)
chris007's avatar
chris007 committed
267
{
andz's avatar
andz committed
268
269
270
    if(pElevation >= ELEV_MIN && pElevation <= ELEV_MAX)
    {
        m_uSetElevation = pElevation;
Markus Mueller's avatar
Markus Mueller committed
271
        m_bElevationShouldRotate = 1;
Markus's avatar
Markus committed
272
        return Rotor::ROT_OK;
andz's avatar
andz committed
273
274
275
    }
    else
    {
Markus's avatar
Markus committed
276
        return Rotor::ROT_VAL_OUT_OF_RANGE;
andz's avatar
andz committed
277
    }
andz's avatar
andz committed
278
279
}

andz's avatar
andz committed
280
281
282
283
284
285
///
/// @brief Reads out the Sensor values and stores them
///
/// @return Error Code
///

Markus's avatar
Markus committed
286
Rotor::rotorError Rotor::readRotSensors()
andz's avatar
andz committed
287
{
Markus Mueller's avatar
Markus Mueller committed
288
289
290
291
    if(AZIM)
      m_uActualAzimuth = (uint16)(readADC(AZIM_POT)/AZIM_DEG);
    if(ELEV)
      m_uActualElevation = (uint16)(readADC(ELEV_POT)/ELEV_DEG);
andz's avatar
andz committed
292
    
Markus's avatar
Markus committed
293
    return Rotor::ROT_OK;
andz's avatar
andz committed
294
295
}

andz's avatar
andz committed
296
297
298
299
300
301
302
303
///
/// @brief Reads out an ADC Pin 4 times and returns the average
///
/// @param AdcPin ADC Pin to read
///
/// @return average ADC value
///

Markus's avatar
Markus committed
304
uint16 Rotor::readADC(int AdcPin)
andz's avatar
andz committed
305
306
{
    //Read ADC 4 times and calculate average
Markus Mueller's avatar
Markus Mueller committed
307
308
    uint32_t adcValue = 0;
    for(int i = 0; i < 128; i++)
andz's avatar
andz committed
309
        adcValue += analogRead(AdcPin);
Markus Mueller's avatar
Markus Mueller committed
310
311
312
313
        delay(1);
    adcValue = adcValue/128;
    return (uint16)adcValue;
}