// run moter
void Motor::run(float power) {
if (power > 1) power = 1;
if (power < -1) power = -1;
if (power < 0) {
power_level->write(-power * limit);
rotate_reverse();
} else {
power_level->write(power * limit);
rotate_normal();
}
}
void positionPID() {
angularPos = angularRes*encoderCount;
position = angularPos*spoolRadius;
error = setPoint - position;
control = Kp*error + Kd*(error-lastError);
lastError = error;
lastPosition = position;
if ((position > 22)||(position < -12)) {
_enable.write(0.00f);
} else {
if (control >= 0.00f) {
if (control > 1.00f) {
pwmControl = 1.00f;
} else {
pwmControl = control;
}
_phase.write(1);
if (pwmControl > 0.00f) {
_enable.write(pwmControl);
} else {
_enable.write(0.00f);
}
} else {
if (fabs(control) > 1.00f) {
pwmControl = 1.00f;
} else {
pwmControl = fabs(control);
}
_phase.write(0);
if (pwmControl > 0.00f) {
_enable.write(pwmControl);
} else {
_enable.write(0.00f);
}
}
}
}
// brake
void Motor::brake(float powor) {
normal = 1;
reverse = 1;
power_level->write(powor * limit);
}
// brake
void Motor::brake(void) {
normal = 1;
reverse = 1;
powerLevel->write(limit);
}