17 lines
383 B
C
17 lines
383 B
C
|
#include "avr.h"
|
||
|
|
#include "util.h"
|
||
|
|
|
||
|
|
void setMotorSpeed(MOTOR *motor, int speed) {
|
||
|
|
if(speed > 0) {
|
||
|
|
*motor->port &= ~motor->revMask;
|
||
|
|
*motor->port |= motor->fwdMask;
|
||
|
|
}
|
||
|
|
else if(speed < 0) {
|
||
|
|
*motor->port &= ~motor->fwdMask;
|
||
|
|
*motor->port |= motor->revMask;
|
||
|
|
}
|
||
|
|
else *motor->port |= motor->fwdMask|motor->revMask;
|
||
|
|
|
||
|
|
*motor->pwmPort = CLAMP(0, ABS(speed), 255);
|
||
|
|
}
|