rotor.cpp 6.82 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
///

chris007's avatar
chris007 committed
14
15
CRotor::CRotor()
{
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.
///

andz's avatar
andz committed
23
void CRotor::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
///

andz's avatar
andz committed
61
void CRotor::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
97
98
99
100
101
                rotateUp();
              else
                rotateDown();
            }
        }
        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
106
107
108
109
                rotateUp();
              else
                rotateDown();
            }
        }
andz's avatar
andz committed
110
    }
Markus's avatar
Markus committed
111
112
113
114
115

    if ( !m_bAzimuthShouldRotate && !m_bElevationShouldRotate ) {
        tightenBreakAzimuth();
        tightenBreakElevation();
    }
Markus Mueller's avatar
Markus Mueller committed
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
}

void CRotor::releaseBreakAzimuth()
{
    m_bAzimuthBreakReleased = 1;
    digitalWrite(AZIM_RE_BREAK, LOW);
}

void CRotor::releaseBreakElevation()
{
    m_bElevationBreakReleased = 1;
    digitalWrite(ELEV_RE_BREAK, LOW);
}

void CRotor::tightenBreakAzimuth()
{
    m_bAzimuthBreakReleased = 0;
    digitalWrite(AZIM_RE_BREAK, HIGH);
}

void CRotor::tightenBreakElevation()
{
    m_bElevationBreakReleased = 0;
    digitalWrite(ELEV_RE_BREAK, HIGH);
}

void CRotor::stopAzimuth()
{
    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
150
151
152
153
154
}

void CRotor::stopElevation()
{
    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
158
159
160
161
162
163
164
165
166
}

CRotor::rotorError CRotor::rotateCW()
{
    if ( m_bAzimuthBreakReleased == 0 )
        return CRotor::ROT_BREAK_NOT_RELEASED;
    digitalWrite(AZIM_RE_CW, LOW);
    digitalWrite(AZIM_RE_CCW, HIGH);
    m_bAzimuthCurrentlyRotating = 1;
    return CRotor::ROT_OK;
}
andz's avatar
andz committed
167

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

Markus Mueller's avatar
Markus Mueller committed
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
CRotor::rotorError CRotor::rotateUp()
{
    if ( m_bElevationBreakReleased == 0 )
        return CRotor::ROT_BREAK_NOT_RELEASED;
    digitalWrite(ELEV_RE_UP, LOW);
    digitalWrite(ELEV_RE_DWN, HIGH);
    m_bElevationCurrentlyRotating = 1;
    return CRotor::ROT_OK;
}

CRotor::rotorError CRotor::rotateDown()
{
    if ( m_bElevationBreakReleased == 0 )
        return CRotor::ROT_BREAK_NOT_RELEASED;
    digitalWrite(ELEV_RE_UP, HIGH);
    digitalWrite(ELEV_RE_DWN, LOW);
    m_bElevationCurrentlyRotating = 1;
    return CRotor::ROT_OK;
}


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

andz's avatar
andz committed
203
204
205
206
207
void CRotor::debugOut()
{
    //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
///

andz's avatar
andz committed
220
221
222
223
224
uint16 CRotor::getActualAzimuth() const
{
    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
///

andz's avatar
andz committed
231
232
233
uint16 CRotor::getActualElevation() const
{
    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
///

andz's avatar
andz committed
244
CRotor::rotorError CRotor::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;
andz's avatar
andz committed
250
251
252
253
254
255
        return CRotor::ROT_OK;
    }
    else
    {
        return CRotor::ROT_VAL_OUT_OF_RANGE;
    }
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
///

andz's avatar
andz committed
266
CRotor::rotorError CRotor::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;
andz's avatar
andz committed
272
273
274
275
276
277
        return CRotor::ROT_OK;
    }
    else
    {
        return CRotor::ROT_VAL_OUT_OF_RANGE;
    }
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
///

andz's avatar
andz committed
286
CRotor::rotorError CRotor::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
293
    
    return CRotor::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
///

andz's avatar
andz committed
304
305
306
uint16 CRotor::readADC(int AdcPin)
{
    //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;
}