/**
* @brief Initializes the class, and prepares the
* SmartDashboard.
*/
ServoController::ServoController(Servo *servo) :
BaseController()
{
mServo = servo;
SmartDashboard *s = SmartDashboard::GetInstance();
s->PutString("(SERVO) Value <<", "0.0");
}
// Called just before this Command runs the first time
void RPMHoldingCommand::Initialize() {
printf("RPMHoldingCommand initialize\n");
double gearReduction = 4.67;
double motorRpmMax = 5000;
rpmMaximum = motorRpmMax / gearReduction;
sampleCount = 0;
float pGain = .0021;
float iGain = .0002;
float dGain = 0;
prototypingsubsystem->GetMotorEncoder()->SetPIDSourceParameter(Encoder::kDistance);
controller = new PIDController(pGain, iGain, dGain, rpmSource, prototypingsubsystem->GetMotor());
controller->SetInputRange(-rpmMaximum, rpmMaximum);
controller->SetOutputRange(-1, 1);
SmartDashboard *sd = oi->getSmartDashboard();
sd->PutDouble("RPM Maximum", rpmMaximum);
timer->Reset();
timer->Start();
lastTime = timer->Get();
lastCount = prototypingsubsystem->GetMotorEncoder()->Get();
controller->SetSetpoint(1000);
controller->Enable(); // start calculating PIDOutput values
printf("RPMHoldingCommand initialize complete\n");
}
void TurretClass::SendData()
{
SmartDashboard *Dash = SmartDashboard::GetInstance();
Dash->PutDouble("Gyro",(double)TurretGyro->GetAngle());
Dash->PutDouble("TurrCom", (double)turretcommand);
Dash->PutDouble("TurrTarget",(double)targetTurretPos);
// Dash->PutDouble("Click_Pos",TargetX);
// Dash->PutDouble("Angle_Calc",AngleX);
// Dash->PutInt("Click_Index",ClickNumber);
}
void Autonomous(void)
{
SmartDashboard *Dash = SmartDashboard::GetInstance();
AutonomousContol->Parse("Auto_Test1.txt");
Dash->PutInt("Parsed",1);
AutonomousContol->Execute();
Dash->PutInt("InAuto", 1);
GetWatchdog().SetEnabled(false);
}
RobotDemo(void)
{
rightStick = new Joystick(2); // create the joysticks
leftStick = new Joystick(1);
turretStick = new Joystick(3);
ds = DriverStation::GetInstance();
SmartDashboard *Dash = SmartDashboard::GetInstance();
AutonomousContol = new AutonomousControllerClass(ds);
compressor = new Compressor(14,1); //Pressure Switch, Compressor Relay
compressor->Start();
Dash->PutInt("Initialized", 1);
}
/**
* @brief Runs the core code. Grabs values from the
* SmartDashboard to change the servo values.
*/
void ServoController::Run()
{
SmartDashboard *s = SmartDashboard::GetInstance();
float value = Tools::StringToFloat(s->GetString("(SERVO) Value <<"));
if (value > 1.0) {
value = 1.0;
} else if (value < -1.0) {
value = -1.0;
}
SmartDashboard::GetInstance()->Log(value, "(SERVO) amount fed:");
mServo->Set(value);
SmartDashboard::GetInstance()->Log(mServo->Get(), "(SERVO) amount reported:");
}
// Called repeatedly when this Command is scheduled to run
void RPMHoldingCommand::Execute() {
double countsPerRevolution = 360;
double currentTime = timer->Get();
double timeDifference = currentTime - lastTime;
lastTime = currentTime;
Encoder* motorEncoder = prototypingsubsystem->GetMotorEncoder();
int currentCount = motorEncoder->Get();
int countDifference = currentCount - lastCount;
double rawRPM = countDifference * (60.0 / countsPerRevolution) / timeDifference;
double rpm = GetRPM(rawRPM);
printf("rawRPM %f rpm %f\n", rawRPM, rpm);
rpmSource->inputRPM(rpm);
//rpmSource->inputRPM(rpm);
printf("setpoint %f RPM %f result %f error %f \n", controller->GetSetpoint(), rpm, controller->Get(), controller->GetError());
SmartDashboard *sd = oi->getSmartDashboard();
oi->DisplayEncoder(motorEncoder);
oi->DisplayPIDController(controller);
sd->PutDouble("Count Difference", countDifference);
sd->PutDouble("Current count", currentCount);
sd->PutDouble("Time difference", timeDifference);
sd->PutDouble("Calculated RPM", rpm);
sd->PutDouble("Motor speed", prototypingsubsystem->GetMotor()->Get());
sd->PutDouble("Counts per revolution", countsPerRevolution);
lastCount = currentCount;
}
void DriveTrainClass::SendData()
{
SmartDashboard *Dash = SmartDashboard::GetInstance();
Dash->PutInt("Left Drive Encocder",LeftDriveEnc->Get());
#if PRACTICE_BOT
Dash->PutInt("Right Drive Encocder",(int)(2.5*RightDriveEnc->Get()));
#else
Dash->PutInt("Right Drive Encocder",(int)(RightDriveEnc->Get()));
#endif
Dash->PutDouble("Left Drive Command",(double)LeftDrive_a->Get());
Dash->PutDouble("Right Drive Command",(double)RightDrive_a->Get());
// Dash->PutString("Gear",gear);
}
void IntakeClass::SendData()
{
SmartDashboard *Dash = SmartDashboard::GetInstance();
Dash->PutString("Uptake",UptakeStatus);
}
void UpdateDash()
{
SmartDashboard *Dash = SmartDashboard::GetInstance();
Dash->PutInt("dashworks",207);
AutonomousContol->SendData();
}
