Arduino_Robot/Motor.cpp at master · Christiaan2/Arduino_Robot · 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
//
//
//

#include "Motor.h"

Motor::Motor(int directionPin, int enablePin, int encoderPin, float Kp, float Kd)
	: directionPin(directionPin), enablePin(enablePin), PWM_val(0), mDirection(true), encoder(encoderPin), speedController(Kp, Kd)
{
	pinMode(this->directionPin, OUTPUT);
	pinMode(this->enablePin, OUTPUT);
	analogWrite(this->enablePin, PWM_val);
	setDirection(mDirection);
}

void Motor::setDirection(bool direction)
{
	mDirection = direction;
	digitalWrite(directionPin, mDirection);
}

void Motor::setSpeed(int speed)
{
	this->speed = speed;
}

void Motor::setDistance(int distance)
{
	this->distance = distance;
}

Motor* Motor::getPointer()
{
	return this;
}

void Motor::setPWM_val(int PWM_val)
{
	this->PWM_val = constrain(PWM_val,0,255);
	analogWrite(enablePin, this->PWM_val);
}

void Motor::driveConstantSpeed(int speed)
{
	this->speed = speed;
	setPWM_val(speedController.calcPidTerm(speed, encoder.calcSpeed()));
}

int Motor::calcPWM_val()
{
	return speedController.calcPidTerm(speed, encoder.calcSpeed());
}

int Motor::getSpeed()
{
	return speed;
}

Encoder* Motor::getPointerToEncoder()
{
	return encoder.getPointer();
}

int Motor::getPWM_val()
{
	return PWM_val;
}

int Motor::getDistance()
{
	return distance;
}

void Motor::reset()
{
	encoder.reset();
	speedController.reset();
	setPWM_val(0);
	speed = 0;
}