30 lines
549 B
C++
30 lines
549 B
C++
|
#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);
|
||
|
|
}
|