euclid/Motor.cpp at master · theDataSmith/euclid · 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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
/*
--FILE--
Motor.cpp

--AUTHOR--
Name:		Josh Alan
GitHub:		theDataSmith
E-mail:		thedatasmith1@gmail.com

--PROJECT--
Euclid, the line-following robot.
GitHub:		github.com/theDataSmith/euclid

--CREATION DATE--
11 / 26 / 2016
*/

#include "Motor.h"
#include <Arduino.h>

namespace EuclidRobot
{
	Motor::Motor(int pinFwd, int pinBwd)
		: pinFwd(pinFwd), pinBwd(pinBwd)
	{
		pinMode(pinFwd, OUTPUT);
		pinMode(pinBwd, OUTPUT);
	}

	void Motor::setSpeed(float speed)
	{
		if (brakeMode)
		{
			//BRAKE mode:

			if (speed >= 0)
			{
				//Forward:

				analogWrite(pinFwd, 255);
				analogWrite(pinBwd, 255 * (1 - speed));
			}
			else
			{
				//Backward:

				analogWrite(pinFwd, 255 * (1 + speed));
				analogWrite(pinBwd, 255);
			}
		}
		else
		{
			//COAST mode:

			if (speed >= 0)
			{
				//Forward:

				analogWrite(pinFwd, 255 * (speed));
				analogWrite(pinBwd, 0);
			}
			else
			{
				//Backward:

				analogWrite(pinFwd, 0);
				analogWrite(pinBwd, 255 * (-speed));
			}
		}
	}

	void Motor::setSpeed(float speed, MotorMode mode)
	{
		setMode(mode);
		setSpeed(speed);
	}

	void Motor::setMode(MotorMode mode)
	{
		brakeMode = bool(mode);
	}
}