Robot() : robotDrive(frontLeftChannel, rearLeftChannel, frontRightChannel, rearRightChannel), // initialize variables in same stick(joystickChannel) // order as declared above. { rotateToAngleRate = 0.0f; robotDrive.SetExpiration(0.1); robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // invert left side motors robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // change to match your robot try { /* Communicate w/navX MXP via the MXP SPI Bus. */ /* Alternatively: I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */ /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ ahrs = new AHRS(SPI::Port::kMXP); } catch (std::exception ex ) { std::string err_string = "Error instantiating navX MXP: "; err_string += ex.what(); DriverStation::ReportError(err_string.c_str()); } turnController = new PIDController(kP, kI, kD, kF, ahrs, this); turnController->SetInputRange(-180.0f, 180.0f); turnController->SetOutputRange(-1.0, 1.0); turnController->SetAbsoluteTolerance(kToleranceDegrees); turnController->SetContinuous(true); /* Add the PID Controller to the Test-mode dashboard, allowing manual */ /* tuning of the Turn Controller's P, I and D coefficients. */ /* Typically, only the P value needs to be modified. */ LiveWindow::GetInstance()->AddActuator("DriveSystem", "RotateController", turnController); if ( ahrs ) { LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs); } }
int main(int argc, char *argv[]) { PIDController* pid = new PIDController(K_P,K_I,K_D); pid->on(); // Turn PID controller on pid->targetSetpoint(SETPOINT); // Change desired setpoint to SETPOINT double t = 0; // Init with time t=0 double T = 0.01; // Period double controlVariable = 0; // Init with zero actuation double processVariable = 0; // Init with zero position cout << "Simulation running..." << endl; ofstream writeToCsv; writeToCsv.open("PIDexample.csv"); writeToCsv << "Time,Setpoint,Output" << endl; while(t < 20) { writeToCsv << t << "," << pid->getSetpoint() << "," << processVariable << endl; controlVariable = pid->calc(processVariable); // Calculate next controlVariable processVariable = LaplaceInversion(xferFn, t, 1e-8); // Simulate plant with TF 1/((s+a)(s+b)) usleep(T*pow(10,6)); // 100ms delay to simulate actuation time t += T; // Increment time variable by 100ms } writeToCsv.close(); cout << "Simulation complete! Output saved to PIDexample.csv." << endl; return 0; }
void UpdateDashboard() { float r = 0.00001 * i; SmartDashboard::PutNumber("State", currentState + r); SmartDashboard::PutNumber("PID Turn Error", turnController->GetError() + r); SmartDashboard::PutNumber("PID Target", turnController->GetSetpoint() + r); // SmartDashboard::PutBoolean("Straight", straight); SmartDashboard::PutData("test", turnController); SmartDashboard::PutNumber("Yaw:", ahrs->GetYaw() + r); SmartDashboard::PutNumber("Roll:", ahrs->GetRoll() + r); SmartDashboard::PutNumber("Pitch", ahrs->GetPitch() + r); SmartDashboard::PutNumber("Scissor 1", pdp->GetCurrent(1) + r); SmartDashboard::PutNumber("Scissor 2", pdp->GetCurrent(2) + r); SmartDashboard::PutNumber("Left Drive 1", pdp->GetCurrent(12) + r); SmartDashboard::PutNumber("Left Drive 2", pdp->GetCurrent(13) + r); SmartDashboard::PutNumber("Right Drive 1", pdp->GetCurrent(14) + r); SmartDashboard::PutNumber("Right Drive 2", pdp->GetCurrent(15) + r); SmartDashboard::PutNumber("Constant Lift", constantLift); SmartDashboard::PutNumber("Rotate Rate", rotateRate + r); i = (i + 1) % 2; printf("2.1"); // .PutLong("test1.2", 1337); printf("2.2"); // mqServer.PutDouble("test",DriverStation::GetInstance().GetMatchTime()); printf("2.3"); // mqServer.PutString("test1.1","YOLO_SWAGINS"); printf("2.4"); // SmartDashboard::PutString("test1.2", mqServer.GetString("test1.1")); // SmartDashboard::PutNumber("test1", mqServer.GetDouble("test")); // SmartDashboard::PutNumber("test1.3", mqServer.GetLong("test1.2")); // SmartDashboard::PutNumber("test2", DriverStation::GetInstance().GetMatchTime()); }
int main(int argc, char** argv) { Motor* motor = new Motor(0); Encoder* encoder = new Encoder(motor); PIDController* pidController = new PIDController(motor, encoder, POSITION_REV, 1, 0, 0); pidController->setSetpoint(10); pidController->enable(); Motor* motor2 = new Motor(1); Encoder* encoder2 = new Encoder(motor2); PIDController* pidController2 = new PIDController(motor2, encoder2, SPEED, 0, 0, 0, .4/20); pidController2->setSetpoint(20); pidController2->enable(); int ticks = 0; while(!pidController->onTarget()) { pidController->update(); pidController2->update(); std::cout << "tick:\t" << ++ticks << "\tcurrent position:\t" << encoder->getPosition() << "\tcurrent speed:\t" << encoder->getSpeed() << "\n"; std::cout << "tick:\t" << ticks << "\tcurrent speed:\t" << encoder2->getSpeed() << "\n"; } std::cout << "done with position " << encoder->getPosition() << "\n"; return 0; }
static void pidInterrupt(void* object) { PIDController* pid = object; if(pid->enabled) { // process the PID data // pid->data.input = pid->pidInput(pid->state); PID_calculate(&pid->data); pid->pidOutput(pid->state, pid->data.output); } }
void AutonomousGyroTurn() { switch (currentState) { case 1: timer->Reset(); timer->Start(); //State: Stopped //Transition: Driving State currentState = 2; break; case 2: //State: Driving //Stay in State until 2 seconds have passed--` //Transition: Gyroturn State drive->TankDrive(0.5, 0.5); if (timer->Get() >= 1) { drive->TankDrive(0.0, 0.0); ahrs->ZeroYaw(); currentState = 3; turnController->SetSetpoint(90); turnController->Enable(); } break; case 3: //State: Gyroturn //Stay in state until navx yaw has reached 90 degrees //Transition: Driving State drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate); // if (ahrs->GetYaw() >= 90) { if (turnController->OnTarget()) { drive->TankDrive(0.0, 0.0); currentState = 4; timer->Reset(); timer->Start(); } break; case 4: //State:Driving //Stay in state until 2 seconds have passed //Transition: State Stopped drive->TankDrive(0.5, 0.5); if (timer->Get() >= 1) { currentState = 5; timer->Stop(); } break; case 5: //State: Stopped drive->TankDrive(0.0, 0.0); break; } }
void ShooterArm::SetTargetAngle(float tgtAngle) { _targetAngle = tgtAngle; _onStage2 = false; _onStage3 = false; PIDController *pid = GetPIDController(); pid->SetPID(_bothStage_P,_stage_1_I,_stage_1_D); GetPIDController()->SetAbsoluteTolerance(fabs(_stage_1_tolerance * VOLTAGE_DEG_SCALAR)); _voltageTarget = DEGREES_TO_VOLTAGE(tgtAngle); GetPIDController()->SetSetpoint(_voltageTarget); Enable(); }
void Disabled() { printf("AAAAAAAAAAAAAAAAAAAAAAAA"); decel = false; done = false; theLift->Reset(); }
float DistToSetpoint() { if(!(controlLift->IsEnabled())) return UNINIT_VAL; else return (liftEncoder->GetDistance() - pidPosSetPoint); }
bool AtSetpoint() { if(!(controlLift->IsEnabled())) return false; else return (abs(DistToSetpoint()) < PID_POS_TOL*LIFT_ENCODER_DIST_PER_PULSE); }
void TeleopPeriodic() { if(m_driver->GetRawButton(BUTTON_LB)) { // PYRAMID m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT); m_PIDController->Enable(); } else if(m_driver->GetRawButton(BUTTON_RB)) { // FEEDER m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT); m_PIDController->Enable(); } else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) { m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB); m_PIDController->Enable(); } else { // MANUAL CONTROL m_PIDController->Disable(); m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y))); m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y))); } // ----- PRINT ----- SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage()); SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet()); } // TeleopPeriodic()
void AutonomousAdjustableStraight() { switch (currentState) { case 1: timer->Reset(); timer->Start(); turnController->Reset(); turnController->SetSetpoint(ahrs->GetYaw()); turnController->Enable(); currentState = 2; break; case 2: intakeLever->Set(autoIntakeSpeed); if (timer->Get() >= 1) { intakeLever->Set(0); currentState = 3; timer->Reset(); timer->Start(); } break; case 3: drive->TankDrive(autoSpeed, autoSpeed); intakeLever->Set(-0.1); if (timer->Get() >= autoLength) { intakeLever->Set(0.0); drive->TankDrive(0.0, 0.0); currentState = 4; timer->Reset(); timer->Start(); } break; case 4: intake->Set(0.5); shooter->Set(-0.5); if (timer->Get() >= 2) { currentState = 5; } break; case 5: intake->Set(0.0); shooter->Set(0.0); drive->TankDrive(0.0, 0.0); break; } }
/** * Uses a PIDController and an array of setpoints to switch and maintain elevator positions. * The elevator setpoint is selected by a joystick button. */ void OperatorControl() { pidController->SetInputRange(0, 5); //0 to 5V pidController->SetSetpoint(setPoints[0]); //set to first setpoint int index = 0; bool currentValue; bool previousValue = false; while (IsOperatorControl() && IsEnabled()) { pidController->Enable(); //begin PID control //when the button is pressed once, the selected elevator setpoint is incremented currentValue = joystick->GetRawButton(buttonNumber); if (currentValue && !previousValue) { pidController->SetSetpoint(setPoints[index]); index = (index + 1) % (sizeof(setPoints)/8); //index of elevator setpoint wraps around } previousValue = currentValue; } }
void RobotInit() override { lw = LiveWindow::GetInstance(); drive->SetExpiration(20000); drive->SetSafetyEnabled(false); //Gyroscope stuff try { /* Communicate w/navX-MXP via the MXP SPI Bus. */ /* Alternatively: I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */ /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ ahrs = new AHRS(SPI::Port::kMXP); } catch (std::exception ex) { std::string err_string = "Error instantiating navX-MXP: "; err_string += ex.what(); //DriverStation::ReportError(err_string.c_str()); } if (ahrs) { LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs); ahrs->ZeroYaw(); // Kp Ki Kd Kf PIDSource PIDoutput turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f, ahrs, this); turnController->SetInputRange(-180.0f, 180.0f); turnController->SetOutputRange(-1.0, 1.0); turnController->SetAbsoluteTolerance(2); //tolerance in degrees turnController->SetContinuous(true); } chooser.AddDefault("No Auto", new int(0)); chooser.AddObject("GyroTest Auto", new int(1)); chooser.AddObject("Spy Auto", new int(2)); chooser.AddObject("Low Bar Auto", new int(3)); chooser.AddObject("Straight Spy Auto", new int(4)); chooser.AddObject("Adjustable Straight Auto", new int(5)); SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT); SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT); SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT); SmartDashboard::PutData("Auto Modes", &chooser); liftdown->Set(false); comp->Start(); }
void AutonomousStraightSpy() { switch (currentState) { case 1: timer->Reset(); timer->Start(); turnController->Reset(); turnController->SetSetpoint(ahrs->GetYaw()); turnController->Enable(); currentState = 2; break; case 2: intakeLever->Set(0.25); if (timer->Get() >= 1) { intakeLever->Set(0); currentState = 3; timer->Reset(); timer->Start(); } break; case 3: drive->TankDrive(0.5, 0.5); if (timer->Get() >= 5) { drive->TankDrive(0.0, 0.0); currentState = 4; timer->Reset(); timer->Start(); } break; case 4: intake->Set(0.5); if (timer->Get() >= 2) { currentState = 5; } break; case 5: intake->Set(0.0); drive->TankDrive(0.0, 0.0); break; } }
Robot() { #if BUILD_VERSION == COMPETITION liftMotor = new CANTalon(CHAN_LIFT_MOTOR); liftMotor2 = new CANTalon(CHAN_LIFT_MOTOR2); #else liftMotor = new CANJaguar(CHAN_LIFT_MOTOR); liftMotor2 = new CANJaguar(CHAN_LIFT_MOTOR2); #endif liftEncoder = new Encoder(CHAN_LIFT_ENCODER_A, CHAN_LIFT_ENCODER_B, false, Encoder::EncodingType::k4X); liftEncoder->SetDistancePerPulse(LIFT_ENCODER_DIST_PER_PULSE); liftEncoder->SetPIDSourceParameter(liftEncoder->kDistance); #if BUILD_VERSION == COMPETITION liftEncoder->SetReverseDirection(true); #endif controlLift = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor); controlLift->SetContinuous(true); //treat input to controller as continuous; true by default controlLift->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX); controlLift->Disable(); //do not enable until in holding position mode controlLift2 = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor2); controlLift2->SetContinuous(true); //treat input to controller as continuous; true by default controlLift2->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX); controlLift2->Disable(); //do not enable until in holding position mode liftLimitSwitchMin = new DigitalInput(CHAN_LIFT_LOW_LS); liftLimitSwitchMax = new DigitalInput(CHAN_LIFT_HIGH_LS); joystick = new Joystick(CHAN_JS); }
int main(int argc, char** argv) { ros::init(argc,argv,"mk_behaviors"); ros::NodeHandle nh; ros::Subscriber vispose_sub = nh.subscribe<mk_msgs::ARMarker>("/mikrokopter/vision/ar_pose_marker", 1000, &getVisualPos); ros::Subscriber sensororient = nh.subscribe<mk_msgs::sensorData>("/mikrokopter/sensor/data",1000,&getSensorOrient); ros::Subscriber ugvposition = nh.subscribe<geometry_msgs::Point>("/mikrokopter/desired",1000,&getUGVPose); ros::Publisher motorcomout = nh.advertise<mk_msgs::motorCommands>("/mikrokopter/commands/motor", 5); double desiredZ = atof(argv[3]); vispose.position.x = 0; vispose.position.y = 0; vispose.position.z = 0; vispose.orientation.x = 1; vispose.orientation.y = 0; vispose.orientation.z = 0; vispose.orientation.w = 0; PIDController pidcontroller; pidcontroller.setDesiredRPY(-1.57); pidcontroller.setPIDParameters(atof(argv[4]),atof(argv[5]),atof(argv[6])); pidcontroller.setForces(2.2, atof(argv[7]), atof(argv[8])); while(ros::ok()){ pidcontroller.setCurrentPosition(vispose.position.x, vispose.position.y, vispose.position.z, roll, pitch, tf::getYaw(vispose.orientation)); pidcontroller.setTime(ros::Time::now().toNSec()); double DesiredPosModRadius = 4; double DesiredPosModFreq = 0.1; double DesiredPosModStartTime = 0; desiredX += 0.1; -DesiredPosModRadius*cos(2*M_PI*DesiredPosModFreq*(DesiredPosModStartTime - ros::Time::now().toSec())) + DesiredPosModRadius; desiredY = 1;-DesiredPosModRadius*sin(2*M_PI*DesiredPosModFreq*(DesiredPosModStartTime - ros::Time::now().toSec())); desiredZ = - 10; pidcontroller.setDesiredPosition(desiredX, desiredY, desiredZ); motorcomout.publish(pidcontroller.pid()); ros::Duration(0.05).sleep(); ros::spinOnce(); } return 0; }
/* ....................................................................... */ void SliderServoMotor::operator()(ODEObject* object) { SliderJoint* slider = object->asSliderJoint() ; PS_ASSERT1( slider != NULL ) ; World* world = object->getWorld() ; PS_ASSERT1( world != NULL ) ; PIDController* pid = getPIDController() ; ooReal vel = 0.0 ; if( pid ) { vel = pid->solve( m_position - slider->getPosition(), world->getCurrentStepSize() ) ; } else { vel = m_position - slider->getPosition() ; } if( m_max_vel >= 0.0 ) { vel = osg::clampTo( vel, -m_max_vel, m_max_vel ) ; } slider->setParam( dParamFMax, m_force ) ; slider->setParam( dParamVel, vel ) ; traverse(object) ; }
void ShooterArm::UsePIDOutput(double output) { //Go from stage 1 to to 2, or from stage 3 to 2 if off target if (((_onStage2 == false) && (OnTarget())) || ((_onStage3 == true) && (OnTarget() == false))) { _onStage2 = true; _onStage3 = false; PIDController *pid = GetPIDController(); pid->SetPID(_bothStage_P,_stage_2_I,_stage_2_D); pid->SetAbsoluteTolerance(fabs(_stage_2_tolerance * VOLTAGE_DEG_SCALAR)); } else if ((_onStage2 == true) && (_onStage3 == false) && (OnTarget())) { _onStage3 = true; PIDController *pid = GetPIDController(); pid->SetPID(_bothStage_P,0.0,_stage_2_D); //pid->SetAbsoluteTolerance(fabs(_stage_2_tolerance * VOLTAGE_DEG_SCALAR)); } // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT shooterArmMotor->PIDWrite(output); // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=OUTPUT }
void Disabled(void) { bool stopped = false; while(!IsOperatorControl() && !IsAutonomous()) { if(!stopped) { frontLeftMotor->Set(0.0); rearLeftMotor->Set(0.0); frontRightMotor->Set(0.0); rearRightMotor->Set(0.0); liftMotor1->Set(0.0); liftMotor2->Set(0.0); gripMotor1->Set(0.0); //gripMotor2->Set(0.0); PIDLift->Reset(); PIDDriveLeft->Reset(); PIDDriveRight->Reset(); PIDGrip->Reset(); stopped = true; } } }
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); } }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { bool reset_yaw_button_pressed = stick.GetRawButton(1); if ( reset_yaw_button_pressed ) { ahrs->ZeroYaw(); } bool rotateToAngle = false; if ( stick.GetRawButton(2)) { turnController->SetSetpoint(0.0f); rotateToAngle = true; } else if ( stick.GetRawButton(3)) { turnController->SetSetpoint(90.0f); rotateToAngle = true; } else if ( stick.GetRawButton(4)) { turnController->SetSetpoint(179.9f); rotateToAngle = true; } else if ( stick.GetRawButton(5)) { turnController->SetSetpoint(-90.0f); rotateToAngle = true; } double currentRotationRate; if ( rotateToAngle ) { turnController->Enable(); currentRotationRate = rotateToAngleRate; } else { turnController->Disable(); currentRotationRate = stick.GetTwist(); } try { /* Use the joystick X axis for lateral movement, */ /* Y axis for forward movement, and the current */ /* calculated rotation rate (or joystick Z axis), */ /* depending upon whether "rotate to angle" is active. */ robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), currentRotationRate ,ahrs->GetAngle()); } catch (std::exception ex ) { std::string err_string = "Error communicating with Drive System: "; err_string += ex.what(); DriverStation::ReportError(err_string.c_str()); } Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
void stateAiming() { stateTimer++; if (stateTimer == 1) { // calculate launcher angle double angle = calcLaunchAngle(m_targets[0].block.y); LOGGER(INFO) << "[stateAiming] Setting Launch Angle: " << angle; if (angle > 46.0) { LOGGER(ERROR) << "[stateAiming] Target is out of range"; robotState = kOperatorControl; return; } launchController->SetSetpoint(angle); launchController->Enable(); return; } else if (stateTimer < 5) { return; } else if (stateTimer < 150) { LOGGER(DEBUG) << "[stateAiming] Timer: " << stateTimer << " SetPoint: " << launchController->GetSetpoint() << " Angle: " << launchPIDSource.PIDGet() << " Correction: " << launchPIDOutput.correction; elevator->Set(-launchPIDOutput.correction); if ((fabs(launchPIDOutput.correction) < 0.2) && (fabs(launchPIDSource.PIDGet()-launchController->GetSetpoint()) < 0.25)) { elevator->Set(0.0); launchController->Disable(); robotState = kLaunching; stateTimer = 0; LOGGER(DEBUG) << "[stateAiming] Target x: " << m_targets[0].block.x << " y: " << m_targets[0].block.y << " h: " << m_targets[0].block.height << " w: " << m_targets[0].block.width; } } else { launchController->Disable(); elevator->Set(0.0); robotState = kOperatorControl; LOGGER(ERROR) << "[stateAiming] Aiming Failed, Timer: " << stateTimer << " Correction: " << launchPIDOutput.correction; } }
void stateCentering() { stateTimer++; if (stateTimer == 1) { turnController->SetSetpoint(0.0); turnController->Enable(); return; } else if (stateTimer < 10) { return; } else if (stateTimer < 120) { if (!turnPIDSource->acquired()) { robotState = kOperatorControl; turnController->Disable(); turnPIDSource->reset(); LOGGER(ERROR) << "[stateCentering] No Target Found"; return; } drive->ArcadeDrive(0.0, -turnPIDOutput.correction, false); LOGGER(DEBUG) << "[stateCentering] Timer: " << stateTimer << " SetPoint: " << turnController->GetSetpoint() << " Offset: " << turnPIDSource->PIDGet() << " Correction: " << turnPIDOutput.correction; if ((fabs(turnPIDOutput.correction) < 0.10) && (fabs(turnPIDSource->PIDGet()) < 3)) { drive->ArcadeDrive(0.0, 0.0, false); turnController->Disable(); robotState = kAiming; stateTimer = 0; turnPIDSource->reset(); } } else { turnController->Disable(); turnPIDSource->reset(); drive->ArcadeDrive(0.0, 0.0, false); robotState = kOperatorControl; LOGGER(ERROR) << "[stateCentering] FAILED, Timer: " << stateTimer << " SetPoint: " << turnController->GetSetpoint() << " Offset: " << turnPIDSource->PIDGet() << " Correction: " << turnPIDOutput.correction; } }
void resetPIDControllers() { delete launchController; delete gatherController; delete turnController; launchController = new PIDController(kLaunchP, kLaunchI, kLaunchD, kLaunchF, &launchPIDSource, &launchPIDOutput); launchController->SetInputRange(kLaunchMinAngle, kLaunchMaxAngle); launchController->SetOutputRange(-1.0, 1.0); launchController->SetAbsoluteTolerance(0.05); launchController->SetContinuous(false); gatherController = new PIDController(kGatherP, kGatherI, kGatherD, kGatherF, &gatherPIDSource, &gatherPIDOutput); gatherController->SetInputRange(0.0, 180.0); gatherController->SetOutputRange(-0.5, 0.5); gatherController->SetAbsoluteTolerance(0.1); gatherController->SetContinuous(false); turnController = new PIDController(kTurnP, kTurnI, kTurnD, kTurnF, turnPIDSource, &turnPIDOutput); turnController->SetInputRange(-160.0, 160.0); turnController->SetOutputRange(-0.4, 0.4); turnController->SetAbsoluteTolerance(0.05); turnController->SetContinuous(false); }
void stateOperatorControl() { // DRIVING move = stick->GetRawAxis(1) * -1.0; rotate = stick->GetRawAxis(4) * -1.0; // Deadband if (fabs(move) < 0.1) { move = 0.0; } if (fabs(rotate) < 0.15) { rotate = 0.0; } drive->ArcadeDrive(move, rotate, false); // Joystick Buttons bool button4 = stick->GetRawButton(4); bool button1 = stick->GetRawButton(1); bool button5 = stick->GetRawButton(5); bool button6 = stick->GetRawButton(6); bool button3 = stick->GetRawButton(3); // Manual Gatherer if (stick->GetRawAxis(2) != 0) { gatherSpeed = stick->GetRawAxis(2); LOGGER(DEBUG) << "[stateOperatorControl] Gather Angle:" << gatherPIDSource.PIDGet(); } else if (stick->GetRawAxis(3) != 0) { gatherSpeed = stick->GetRawAxis(3) * -1; LOGGER(DEBUG) << "[stateOperatorControl] Gather Angle:" << gatherPIDSource.PIDGet(); } else { gatherSpeed = 0.0; } gatherer->Set(gatherSpeed); // Launch Angle double launcherAngle = launchPIDSource.PIDGet(); if (button5 && !button6 && (launcherAngle < kLaunchMaxAngle)) { elevator->Set(-0.5); // Up LOGGER(DEBUG) << "[stateOperatorControl] Launcher Angle:" << launcherAngle; } else if (button6 && !button5 && (launcherAngle > kLaunchMinAngle)) { LOGGER(DEBUG) << "[stateOperatorControl] Launcher Angle:" << launcherAngle; elevator->Set(0.5); // Down } else { elevator->Set(0.0); } // Auto-Gather if (button3 && !lastButton3) { wheelsGathererIn = !wheelsGathererIn; gatherController->SetSetpoint(kGatherAngle); gatherController->Enable(); } if (wheelsGathererIn) { gathererWheels->Set(1.0); gatherer->Set(gatherPIDOutput.correction); LOGGER(DEBUG) << "[stateOperatorControl] Gather Correction:" << gatherPIDOutput.correction << " Gather Angle: " << gatherPIDSource.PIDGet(); } else { gathererWheels->Set(0.0); gatherController->Disable(); } if (button4 && !lastButton4) { stateTimer = 0; robotState = kCentering; shootingHigh = true; } if (button1 && !lastButton1) { stateTimer = 0; robotState = kLaunching; shootingHigh = true; } lastButton4 = button4; lastButton1 = button1; lastButton3 = button3; }
/** * Call the Calculate method as a non-static method. This avoids having to prepend * all local variables in that method with the class pointer. This way the "this" * pointer will be set up and class variables can be called more easily. * This method is static and called by the Notifier class. * @param controller the address of the PID controller object to use in the background loop */ void PIDController::CallCalculate(void *controller) { PIDController *control = (PIDController*) controller; control->Calculate(); }
void reset() { delta_encoder = 0; pwm = 0; pid_controller.reset(); }
void OperatorControl(void) { autonomous = false; //myRobot.SetSafetyEnabled(false); //myRobot.SetInvertedMotor(kFrontLeftMotor, true); // myRobot.SetInvertedMotor(3, true); //variables for great pid double rightSpeed,leftSpeed; float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP; float stickY[2]; float stickYAbs[2]; bool lightOn = false; AxisCamera &camera = AxisCamera::GetInstance(); camera.WriteResolution(AxisCameraParams::kResolution_160x120); camera.WriteMaxFPS(5); camera.WriteBrightness(50); camera.WriteRotation(AxisCameraParams::kRotation_0); rightEncoder->Start(); leftEncoder->Start(); liftEncoder->Start(); rightEncoder->Reset(); leftEncoder->Reset(); liftEncoder->Reset(); bool fastest = false; //transmission mode float reduction = 1; //gear reduction from bool bDrivePID = false; //float maxSpeed = 500; float liftPower = 0; float liftPos = -10; bool disengageBrake = false; int count = 0; //int popCount = 0; double popStart = 0; double popTime = 0; int popStage = 0; int liftCount = 0; int liftCount2 = 0; const float LOG17 = log(17.61093344); float liftPowerAbs = 0; float gripError = 0; float gripErrorAbs = 0; float gripPropMod = 0; float gripIntMod = 0; bool shiftHigh = false; leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution, rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO(); GetWatchdog().SetEnabled(true); GetWatchdog().SetExpiration(0.3); PIDDriveLeft->SetOutputRange(-1, 1); PIDDriveRight->SetOutputRange(-1, 1); //PIDDriveLeft->SetInputRange(-244,244); //PIDDriveRight->SetInputRange(-244,244); PIDDriveLeft->SetTolerance(5); PIDDriveRight->SetTolerance(5); PIDDriveLeft->SetContinuous(false); PIDDriveRight->SetContinuous(false); //PIDDriveLeft->Enable(); //PIDDriveRight->Enable(); PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN); PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN); liftSP = 0; PIDLift->SetTolerance(10); PIDLift->SetContinuous(false); PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG PIDLift->Enable(); gripSP = 0; float goalGripSP = 0; bool useGoalSP = true; bool gripPIDOn = true; float gripP[10]; float gripI[10]; float gripD[10]; PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up PIDGrip->SetTolerance(5); PIDGrip->SetSetpoint(0); PIDGrip->Enable(); miniDep->Set(miniDep->kForward); int i = 0; while(i < 10) { gripP[i] = GRIPPROPGAIN; i++; } i = 0; while(i < 10) { gripI[i] = GRIPINTGAIN; i++; } i = 0; while(i < 10) { gripD[i] = GRIPDERIVGAIN; i++; } while (IsOperatorControl()) { GetWatchdog().Feed(); count++; #if !(SKELETON) sendVisionData(); #endif /* if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n"); if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n"); if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n"); if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n"); if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n"); if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n"); */ /* if(lsLeft->Get()) printf("LSLEFT\n"); if(lsMiddle->Get()) printf("LSMIDDLE\n"); if(lsRight->Get()) printf("LSRIGHT\n"); */ stickY[0] = stickL.GetY(); stickY[1] = stickR.GetY(); stickYAbs[0] = fabs(stickY[0]); stickYAbs[1] = fabs(stickY[1]); if(bDrivePID) { #if 0 frontLeftMotor->Set(stickY[0]); rearLeftMotor->Set(stickY[0]); frontRightMotor->Set(stickY[1]); rearRightMotor->Set(stickY[1]); if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(), rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get()); #endif if(stickYAbs[0] <= 0.05 ) { leftSP = 0; if(!(count%3) && !BACKWARDBUTTON) { PIDDriveLeft->Reset(); PIDDriveLeft->Enable(); } } else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control if(stickYAbs[1] <= 0.05) { rightSP = 0; if(!(count%3) && !BACKWARDBUTTON) { PIDDriveRight->Reset(); PIDDriveRight->Enable(); } } else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]); if (BACKWARDBUTTON) { tempRightSP = rightSP; tempLeftSP = leftSP; rightSP = -tempLeftSP; leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically. } PIDDriveLeft->SetSetpoint(leftSP); PIDDriveRight->SetSetpoint(rightSP); leftSpeed = leftEncoder->GetRate(); rightSpeed = rightEncoder->GetRate(); if(!(count++ % 5)) { printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP, PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(), frontRightMotor->Get()); //printf("Throttle value: %f", stickR.GetThrottle()); if(PIDDriveRight->OnTarget()) printf("Right on \n"); if(PIDDriveLeft->OnTarget()) printf("Left on \n"); } if(PIDRESETBUTTON) { //PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); //PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); PIDDriveLeft->Reset(); PIDDriveRight->Reset(); PIDDriveLeft->Enable(); PIDDriveRight->Enable(); } } else { if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset(); if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset(); if(DEMOSWITCH) { stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height stickY[1] = stickY[0]*(1 - lift->getPosition()); } if(stickYAbs[0] > 0.05) { frontLeftMotor->Set(stickY[0]); rearLeftMotor->Set(stickY[0]); } else { frontLeftMotor->Set(0); rearLeftMotor->Set(0); } if(stickYAbs[1] > 0.05) { frontRightMotor->Set(-stickY[1]); rearRightMotor->Set(-stickY[1]); } else { frontRightMotor->Set(0); rearRightMotor->Set(0); } } if(stickL.GetRawButton(2) && stickL.GetRawButton(3) && stickR.GetRawButton(2) && stickR.GetRawButton(3) && BACKWARDBUTTON && !(count%5)) bDrivePID = !bDrivePID; if((SHIFTBUTTON && shiftHigh) || DEMOSWITCH) { shifter->Set(shifter->kReverse); shiftHigh = false; maxSpeed = 12; } else if(!SHIFTBUTTON && !shiftHigh && !DEMOSWITCH) { shifter->Set(shifter->kForward); shiftHigh = true; maxSpeed = 25; //last value 35 } sendIOPortData(); #if !(SKELETON) /* if(LIGHTBUTTON) lightOn = true; else lightOn = false; if(!lightOn) light->Set(light->kOff); if(lightOn) light->Set(light->kOn); */ if(!MODESWITCH) { lastLiftSP = liftSP; if(!PIDLift->IsEnabled()) PIDLift->Enable(); if(LIFTLOW1BUTTON) liftSP = LIFTLOW1; if(LIFTLOW2BUTTON) liftSP = LIFTLOW2; if(LIFTMID1BUTTON) liftSP = LIFTMID1; if(LIFTMID2BUTTON) liftSP = LIFTMID2; if(LIFTHIGH1BUTTON) liftSP = LIFTHIGH1; if(LIFTHIGH2BUTTON && !(DEMOSWITCH && (stickYAbs[0] > 0.05 || stickYAbs[1] > 0.05))) liftSP = LIFTHIGH2; if(!lift->isBraking() && !disengageBrake) { PIDLift->SetSetpoint(liftSP); if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG { //PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN); PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG } else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG } if(lift->isBraking() && lastLiftSP != liftSP) { PIDLift->SetSetpoint(lastLiftSP + 0.06); PIDLift->SetPID(12.5 + 1.5*lift->getPosition(), LIFTINTGAIN + 0.6 + 3*lift->getPosition()/10, 0); lift->brakeOff(); disengageBrake = true; if(!liftCount) liftCount = count; } //set brake (because near) if(fabs(PIDLift->GetError()) < 0.01 && !lift->isBraking() && !disengageBrake) { if(liftCount == 0) liftCount = count; if(count - liftCount > 3) { PIDLift->Reset(); liftMotor1->Set(LIFTNEUTRALPOWER); liftMotor2->Set(LIFTNEUTRALPOWER); Wait(0.02); lift->brakeOn(); Wait(0.02); liftMotor1->Set(0.0); liftMotor2->Set(0.0); PIDLift->Enable(); PIDLift->SetSetpoint(lift->getPosition()); liftCount = 0; } //if(!(count%50)) printf("Braking/Not PID \n"); } if(lift->isBraking() && !(count%10)) PIDLift->SetSetpoint(lift->getPosition()); if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && count - liftCount > 3) { disengageBrake = false; if(liftEncoder->PIDGet() < liftSP) PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN - 0.015); else PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN + 0.015); liftCount = 0; } //pid /* else if(!(fabs(PIDLift->GetError()) < 0.04) && !lift->isBraking() && liftPos == -20) { PIDLift->Enable(); liftPos = -10; printf("PID GO PID GO PID GO PID GO PID GO \n"); } */ //when liftPos is positive, measures position //when liftPos = -10, is pidding //when liftPos = -20, has just released brake } else //(MODESWITCH) { if(PIDLift->IsEnabled()) PIDLift->Reset(); liftPower = .8*pow(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116), 2)*(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)/fabs(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116))); liftPowerAbs = fabs(liftPower); if(liftPowerAbs > 1) liftPower /= liftPowerAbs; //if(!(count%5)) printf("Slider: %f", liftPower); if(lift->isBraking() && liftPowerAbs > 0.05) lift->brakeOff(); else if(!lift->isBraking() && liftPowerAbs <= 0.05 && !(count%5)) lift->brakeOn(); if (liftPowerAbs > 0.05) { liftMotor1->Set(liftPower); liftMotor2->Set(liftPower); } else { liftMotor1->Set(0); liftMotor2->Set(0); } } if(MODESWITCH && LIFTLOW1BUTTON && LIFTMID1BUTTON && LIFTHIGH1BUTTON) liftEncoder->Reset(); /* if(!(count%5)) { printf("Lift pos: %f Lift error: %f Lift SP: %f \n", liftPIDSource->PIDGet(), PIDLift->GetError(), PIDLift->GetSetpoint()); } */ if(!(count%5) && MODESWITCH && GRIPPERPOSUP && GRIPPERPOSDOWN && GRIPPERPOP) { gripPIDOn = !gripPIDOn; printf("Switch'd\n"); } if(gripPIDOn) { if(!PIDGrip->IsEnabled()) PIDGrip->Enable(); if(GRIPPERPOSUP && !GRIPPERPOSDOWN) { goalGripSP = 0; } else if(GRIPPERPOSDOWN && !GRIPPERPOSUP && lift->getPosition() < 0.5) { goalGripSP = 1; } /* else if(!GRIPPERPOSDOWN && !GRIPPERPOSUP) { goalGripSP = 0.5; } */ gripError = PIDGrip->GetError(); gripErrorAbs = fabs(gripError); PIDGrip->SetSetpoint(goalGripSP); if(gripErrorAbs < 0.4) PIDGrip->SetOutputRange(-0.4, 0.6); //negative is up else PIDGrip->SetOutputRange(-0.9, 0.8); //negative is up if(gripErrorAbs > 0.0 && gripErrorAbs < 0.2) { PIDGrip->SetPID(GRIPPROPGAIN - 1.25*(1 - gripErrorAbs) + gripPropMod, GRIPINTGAIN + gripIntMod, 0.3 + 0.1*(1 - gripPIDSource->PIDGet())); } else { PIDGrip->SetPID(GRIPPROPGAIN - 1.*(1 - gripErrorAbs) + gripPropMod, 0, 0.02); } if(fabs(gripPIDSource->PIDGet()) < 0.03 && PIDGrip->GetSetpoint() == 0) { gripLatch1->Set(true); gripLatch2->Set(false); } else if(!(gripLatch1->Get() && PIDGrip->GetSetpoint() == 0) || gripPIDSource->PIDGet() < 0) { gripLatch1->Set(false); gripLatch2->Set(true); } if(gripLatch1->Get() && !(count%20)) { PIDGrip->Reset(); PIDGrip->Enable(); } /* if(stickL.GetRawButton(1) && !(count%5)){ gripIntMod -= 0.001; } if(stickR.GetRawButton(1) &&!(count%5)) { gripIntMod += 0.001; } if(stickL.GetRawButton(2) && !(count%5)) { gripPropMod -= 0.1; } if(stickL.GetRawButton(3) && !(count%5)) { gripPropMod += 0.1; } */ /* if(LIFTBOTTOMBUTTON) { PIDGrip->Reset(); PIDGrip->Enable(); } */ if(!(count%5)) { printf("Gripper pos: %f Gripper error: %f Grip power: %f \n", gripPIDSource->PIDGet(), PIDGrip->GetError(), gripMotor1->Get()); } } //Calibration routine else { if(gripLatch1->Get() == true) { gripLatch1->Set(false); gripLatch2->Set(true); } if(PIDGrip->IsEnabled()) PIDGrip->Reset(); if(GRIPPERPOSUP) { gripMotor1->Set(-0.5); //gripMotor2->Set(0.5); } else if(GRIPPERPOSDOWN) { gripMotor1->Set(0.5); //gripMotor2->Set(-0.5); } else { gripMotor1->Set(0); //gripMotor2->Set(0); } } //if(!(count%5)) printf("Grip volts: %f \n", gripPot->GetVoltage()); //if(!(count%5)) printf("Grip 1 voltage: %f \n", gripMotor1->Get()); if(GRIPPEROPEN && !popStage) { #if !(INNERGRIP) gripOpen1->Set(true); gripOpen2->Set(false); #else gripOpen1->Set(false); gripOpen2->Set(true); #endif } else if(!popStage) { #if !(INNERGRIP) gripOpen1->Set(false); gripOpen2->Set(true); #else gripOpen1->Set(true); gripOpen2->Set(false); #endif } if(GRIPPERPOP && !popStage && goalGripSP == 0 && !(GRIPPEROPEN && GRIPPERCLOSE)) popStage = 1; if(popStage) popTime = GetClock(); if(popStage == 1) { //popCount = count; popStart = GetClock(); popStage++; //printf("POP START POP START POP START \n"); } if(popStage == 2) { #if !(INNERGRIP) gripOpen1->Set(true); gripOpen2->Set(false); #else gripOpen1->Set(false); gripOpen2->Set(true); #endif popStage++; //printf("GRIP OPEN GRIP OPEN GRIP OPEN \n"); } if(popStage == 3 && popTime - popStart > 0.0) //used to be 0.15 { gripPop1->Set(true); gripPop2->Set(false); popStage++; //printf("POP OUT POP OUT POP OUT \n"); } if(popStage == 4 && popTime - popStart > .75) //time was 0.9 { gripPop1->Set(false); gripPop2->Set(true); popStage++; //printf("POP IN POP IN POP IN \n"); } if(popStage == 5 && popTime - popStart > 0.9) popStage = 0; //time was 1.05 if(MINIBOTSWITCH && !(MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1))) miniDep->Set(miniDep->kReverse); else if(MINIBOTSWITCH && MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1)) miniDep->Set(miniDep->kOn); #endif if(!compSwitch->Get()) compressor->Set(compressor->kReverse); else compressor->Set(compressor->kOff); /* if(stickR.GetRawButton(1)) compressor->Set(compressor->kReverse); else compressor->Set(compressor->kForward); */ Wait(0.02); // wait for a motor update time } }