blob: 706096722ea0de14ccfb4395eb7af057c919f2f1 (
plain)
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
|
#include "Motor.h"
#include "util.h"
Motor::Motor(volatile uint8_t *port, volatile uint8_t *pwmPort, uint8_t fwdMask, uint8_t revMask) {
this->port = port;
this->pwmPort = pwmPort;
this->fwdMask = fwdMask;
this->revMask = revMask;
setSpeed(0);
}
void Motor::setSpeed(int speed) {
this->speed = CLAMP(-255, speed, 255);
if(speed > 0) {
*port &= ~revMask;
*port |= fwdMask;
}
else if(speed < 0) {
*port &= ~fwdMask;
*port |= revMask;
}
else *port |= fwdMask|revMask;
*pwmPort = ABS(this->speed);
}
|