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 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);
}
// 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");
}
// 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;
}
