throttle_controller/motor.cpp at master · dangu/throttle_controller · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#include "Arduino.h"
#include "motor.hpp"

/**@brief Motor class constructor */
Motor::Motor(int pwm, int en1, int en2)
{
  _pwm = pwm;
  _en1 = en1;
  _en2 = en2;
}

/**@brief Initialize motor object */
void Motor::init()
{
  pinMode(_pwm, OUTPUT);
  pinMode(_en1, OUTPUT);
  pinMode(_en2, OUTPUT);
  digitalWrite(_en1, LOW);
  digitalWrite(_en2, LOW);
}

void Motor::forward(int spd)
{
  speed(spd);
}

void Motor::reverse(int spd)
{
  speed(-spd);
}

void Motor::speed(int spd)
{
  digitalWrite(_pwm,1);
  if(spd>0)
    {
      analogWrite(_en1, spd);
      digitalWrite(_en2, LOW);
    }
  else
    {
      digitalWrite(_en1, LOW);
      analogWrite(_en2, -spd);
    }


}
/**@brief Stop motor by shorting the leads */
void Motor::stop()
{
  digitalWrite(_en1, LOW);
  digitalWrite(_en2, LOW);
  digitalWrite(_pwm, 1);
}