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
}
}
