void SetMotorSpeedRight(float speed) {
char myString[64];
if (pidDrive) {
if (fabs(speed) < 0.5) {
speed = 0;
}
sprintf(myString, "pid SP R: %5.2f\n", speed);
SmartDashboard::PutString("DB/String 9", myString);
controlRight->SetSetpoint(speed);
} else {
sprintf(myString, "setpoint R: %5.2f\n", speed);
SmartDashboard::PutString("DB/String 9", myString);
m_pscMotorRight->Set(speed);
}
}
void SetMotorSpeedLeft(float speed) {
char myString[64];
if (pidDrive) {
if (fabs(speed) < 0.5) {
speed = 0;
}
sprintf(myString, "pid SP L: %5.2f\n", -speed);
SmartDashboard::PutString("DB/String 8", myString);
controlLeft->SetSetpoint(-speed);
} else {
sprintf(myString, "setpoint L: %5.2f\n", -speed);
SmartDashboard::PutString("DB/String 8", myString);
m_pscMotorLeft->Set(-speed);
}
}
void PIDWrite(float output) {
motor->Set(motor->Get() + mult * output);
//printf("pid drive enabled");
}
void Set(float _speed, UINT8 syncGroup=0) {
speed = _speed;
if (this->SendablePIDController::IsEnabled()) SendablePIDController::SetSetpoint(speed * scale);
//else motor->Set(speed);
motor->Set(speed);
}
void Reset() {
m_speedController->Set(0.0f);
m_encoder->Reset();
}
void LiftPIDOutput::PIDWrite(float output)
{
m_liftMotor1->Set(output + LIFTNEUTRALPOWER);
m_liftMotor2->Set(output + LIFTNEUTRALPOWER);
}
void GripPIDOutput::PIDWrite(float output)
{
m_motor1->Set(output);
//m_motor2->Set(-output);
}
