motor.cpp 2.36 KB
Newer Older
1
2
#include "motor.h"

markus's avatar
markus committed
3
Motor::Motor(double max, double min, int down_pin, int up_pin, Brake* brake, int sensor_pin, double degree, double resolution, double span)
4
5
6
7
{
    max_ = max;
    min_ = min;
    span_ = span;
markus's avatar
markus committed
8
    brake_ = brake;
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
    degree_ = degree;
    up_pin_ = up_pin;
    down_pin_ = down_pin;
    sensor_pin_ = sensor_pin;
    resolution_ = resolution;

    //Pin Settings
    pinMode(up_pin_, OUTPUT);
    pinMode(down_pin_, OUTPUT);

    //All Relais Off
    digitalWrite(up_pin_, HIGH);
    digitalWrite(down_pin_, HIGH);

    should_rotate_ = false;
markus's avatar
markus committed
24
    brake_tightened_ = true;
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
}

void Motor::doMotor()
{
    double diff;
    readMotSensor();
    if(should_rotate_)
    {
        diff = position_ - destination_;
        if(abs(diff) < resolution_)
            stop();
        else {
            if(diff > 0)
                rotateDown();
            else
                rotateUp();
        }
    }
}

Motor::motorError Motor::rotateUp()
{
markus's avatar
markus committed
47
    releaseBrake();
48
49
50
51
52
53
54
    digitalWrite(down_pin_, HIGH);
    digitalWrite(up_pin_, LOW);
    return Motor::ROT_OK;
}

Motor::motorError Motor::rotateDown()
{
markus's avatar
markus committed
55
    releaseBrake();
56
57
58
59
60
    digitalWrite(up_pin_, HIGH);
    digitalWrite(down_pin_, LOW);
    return Motor::ROT_OK;
}

markus's avatar
markus committed
61
void Motor::releaseBrake()
62
{
markus's avatar
markus committed
63
64
65
66
    if(brake_tightened_) {
        brake_->release();
        brake_tightened_ = false;
    }
67
68
}

markus's avatar
markus committed
69
void Motor::tightenBrake()
70
{
markus's avatar
markus committed
71
72
73
74
    if(brake_tightened_ == false) {
        brake_->tighten();
        brake_tightened_ = true;
    }
75
76
77
78
79
}

void Motor::stop()
{
    should_rotate_ = false;
markus's avatar
markus committed
80
    tightenBrake();
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
    digitalWrite(down_pin_, HIGH);
    digitalWrite(up_pin_, HIGH);
}

double Motor::getPosition() const
{
    return position_;
}

Motor::motorError Motor::setPosition(double destination)
{
    if(destination >= min_ && destination <= max_) {
        destination_ = destination;
        if(abs(destination_ - position_) > span_) {
            should_rotate_ = true;
            return Motor::ROT_OK;
        }
        return Motor::ROT_ALREADY_IN_SPAN;
    }
    return Motor::ROT_VAL_OUT_OF_RANGE;
}

Motor::motorError Motor::readMotSensor()
{
    position_ = readADC(sensor_pin_)/degree_;
    return Motor::ROT_OK;
}

double Motor::readADC(int AdcPin)
{
    //Read ADC 10 times and calculate average
    uint32_t adcValue = 0;
    for(int i = 0; i < 10; i++)
        adcValue += analogRead(AdcPin);
        delay(1);
    return (((double)adcValue)/10);
}