void RobotInit(void) { // Actions which would be performed once (and only once) upon initialization of the // robot would be put here. // Test to see if Dashboard is connected--------------------------------------------------------- // printf("HUE MIN: %d\n", SmartDashboard::GetNumber("HUE MIN")); // Initialize exponent value from SmartDashboard exp = 3.0; buttonWasDown = false; // Initialize settings for encoder drive PIDControllers // Drive_PID_Controller->SetOutputRange(-0.2, 0.2); // Drive_PID_Controller->SetTolerance(0.1); // Initialize settings for RobotTurn // Goal_Align_PID->SetOutputRange(-0.2, 0.2); // Goal_Align_PID->SetTolerance(0.1); // Set encoders Front_R->ConfigEncoderCodesPerRev(360); Front_L->ConfigEncoderCodesPerRev(360); // Set each drive motor to have an encoder to be its friend Front_R->SetPositionReference(CANJaguar::kPosRef_QuadEncoder); Front_L->SetPositionReference(CANJaguar::kPosRef_QuadEncoder); printf("RobotInit() completed.\n"); }
void Disabled (void) { debug("disable"); Dlf->StopMotor(); Dlb->StopMotor(); Drf->StopMotor(); Drb->StopMotor(); }
void Drive (float speed, float turn, float strafe) { Dlf->Set(range(speed + turn + strafe)*250, 2); Dlb->Set(range(speed + turn - strafe)*250, 2); Drf->Set(range(-speed + turn + strafe)*250, 2); Drb->Set(range(-speed + turn - strafe)*250, 2); //CANJaguar::UpdateSyncGroup(2); }
void DoctaEight::leftyrighty(double left, double right)//set drive motors on either side { REDRUM; righty.Set(negate*right*.9); rightyB.Set(negate*right*.9); lefty.Set(negate*left*.9); leftyB.Set(negate*left*.9); }
void setMotors(float left,float right) { left=-left; right=-right; left*=300; right*=300; frontLeft->Set(-left); frontRight->Set(right); backRight->Set(right); backLeft->Set(-left); //CANJaguar::UpdateSyncGroup(2); }
CANRobotDemo() : speedJag(2, CANJaguar::kSpeed) , stick(1) , axis(Joystick::kXAxis) , commandTask("DashboardCommandServer", (FUNCPTR)DashboardCommandServerStub) { GetWatchdog().SetExpiration(100); speedJag.ConfigEncoderCodesPerRev(360); speedJag.ConfigMaxOutputVoltage(6.0); speedJag.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); wpi_stackTraceEnable(true); commandTask.Start((INT32)this); }
void UpdateDashboardStatus() { Dashboard &dashHigh = m_ds->GetHighPriorityDashboardPacker(); dashHigh.AddCluster(); // PID (not used for now) dashHigh.AddDouble(0.0); // P dashHigh.AddDouble(0.0); // I dashHigh.AddDouble(0.0); // D dashHigh.FinalizeCluster(); dashHigh.AddDouble(speedJag.GetSpeed()); // Current position dashHigh.AddDouble(speedJag.Get()); // Setpoint dashHigh.AddDouble(speedJag.GetOutputVoltage()); // Output Voltage dashHigh.Finalize(); }
void OperatorControl(void) { while(!IsDisabled()) { GetWatchdog().Feed(); float speed = stick.GetRawAxis(2); float strafe = -1*stick.GetRawAxis(1); float turn = -1*stick.GetRawAxis(3); Dlf->Set(speed + turn + strafe); Dlb->Set(speed + turn - strafe); Drf->Set(-speed + turn + strafe); Drb->Set(-speed + turn - strafe); Wait(.05); } }
void TeleopInit(void) { m_telePeriodicLoops = 0; // Reset the loop counter for teleop mode m_dsPacketsReceivedInCurrentSecond = 0; // Reset the number of dsPackets in current second // Default autoPilot to off autoPilot = false; Front_R->EnableControl(); Front_L->EnableControl(); // Enable Goal Align PID // Goal_Align_PID->Disable(); // Stop previous enables // Goal_Align_PID->Enable(); // Goal_Align_PID->SetSetpoint(0.0); }
void RawControl::armAngle(float angle) { //angle/=300.0f; arm->Set(angle); //cout<<"angle= "<<angle<<"\n"; }
void AutonomousInit() { init(); currentDistance = 0; goalDistance = 6.0*12.0; // 6 feet autonomousShooterDelay = 0; shooterMotor->Set(0); loaderMotor->Set(Relay::kOff); }
void TeleopPeriodic(void) { // increment the number of teleop periodic loops completed m_telePeriodicLoops++; GetWatchdog().Feed(); // if(autoPilot == true) // { // Auto Align Disable Button // if(operatorGamepad->GetButton(Joystick::kTopButton) == 2) // { // Goal_Align_PID->Disable(); // Stop outputs // Goal_Align_PID->Enable(); // Start PIDContoller up again // Goal_Align_PID->SetSetpoint(0.0); // autoPilot = false; // } // } // else // { // Calculate jaguar output based on exponent we pass from SmartDashboard double leftOutput, rightOutput; leftOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(2)); rightOutput = calculateDriveOutputForTeleop(operatorGamepad->GetRawAxis(5)); m_robotDrive->SetLeftRightMotorOutputs(leftOutput, rightOutput); if(operatorGamepad->GetRawButton(1) && !buttonWasDown) { printf("LEFT ENCODER: %f\n", Front_L->GetPosition()); printf("RIGHT ENCODER: %f\n", Front_R->GetPosition()); } buttonWasDown = operatorGamepad->GetRawButton(1); // Auto Align Button // if(operatorGamepad->GetButton(Joystick::kTopButton) == 1) // { // // Turn Auto Align on if we see a goal and we know the azimuth // if(SmartDashboard::GetBoolean(FOUND_KEY) == true) // { // Goal_Align_PID->SetSetpoint(SmartDashboard::GetNumber(AZIMUTH_KEY)); // autoPilot = true; // } // } // } }
void StartWheels() { if (!spinFastNow) { printf(">>> StartWheels\n"); Log(LOG_START, 0, 0); spinFastNow = true; // start shooter wheels in %vbus mode, max output #ifdef HAVE_TOP_WHEEL #ifdef HAVE_TOP_CAN1 jagVbus(topWheel1, maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(maxOutput); Log(LOG_MODE, 1, 1); #endif #ifdef HAVE_TOP_CAN2 jagVbus(topWheel2, maxOutput); Log(LOG_MODE, 2, 1); #endif #endif #ifdef HAVE_BOTTOM_WHEEL #ifdef HAVE_BOTTOM_CAN1 jagVbus(bottomWheel1, maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(maxOutput); Log(LOG_MODE, 3, 1); #endif #ifdef HAVE_BOTTOM_CAN2 jagVbus(bottomWheel2, maxOutput); Log(LOG_MODE, 4, 1); #endif #endif topPID = bottomPID = false; // reset reporting counter report = 0; printf("<<< StartWheels\n"); } }
/** * Initialization code for test mode should go here. * * Use this method for initialization code which will be called each time * the robot enters test mode. */ void TestInit() { printf(">>> TestInit\n"); #ifdef HAVE_COMPRESSOR compressor->Start(); #endif #ifdef HAVE_ARM arm->Set(DoubleSolenoid::kOff); #endif #ifdef HAVE_INJECTOR injectorL->Set(DoubleSolenoid::kOff); injectorR->Set(DoubleSolenoid::kOff); #endif #ifdef HAVE_EJECTOR ejector->Set(false); #endif #ifdef HAVE_LEGS legs->Set(false); #endif #ifdef HAVE_TOP_WHEEL #ifdef HAVE_TOP_CAN1 jagVbus(topWheel1, 0.0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Set(0.0); #endif #ifdef HAVE_TOP_CAN2 jagVbus(topWheel2, 0.0); #endif #endif #ifdef HAVE_BOTTOM_WHEEL #ifdef HAVE_BOTTOM_CAN1 jagVbus(bottomWheel1, 0.0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Set(0.0); #endif #ifdef HAVE_BOTTOM_CAN2 jagVbus(bottomWheel2, 0.0); #endif #endif printf("<<< TestInit\n"); }
void TeleopPeriodic() { SmartDashboard::PutNumber("joystickX",stick.GetX()); SmartDashboard::PutNumber("joystickY",stick.GetY()); //SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1)); //SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage()); SmartDashboard::PutBoolean("infra",infra.Get()); SmartDashboard::PutNumber("accelX",accel.GetX()); SmartDashboard::PutNumber("accelY",accel.GetY()); SmartDashboard::PutNumber("accelZ",accel.GetZ()); servo.Set( trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick ); SmartDashboard::PutNumber("servo", servo.Get()); jag1.Set(stick.GetY()); jag2.Set(stick.GetY()); //tal1.Set(stick.GetY()); SmartDashboard::PutNumber("jag1", jag1.Get()); SmartDashboard::PutNumber("jag2", jag2.Get()); /*SmartDashboard::PutNumber("encpos", enc.Get()); SmartDashboard::PutNumber("encspd", enc.GetRate());*/ if (stick.GetRawButton(1) && !actuatePressed) { pistonVal=!pistonVal; piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse); actuatePressed = true; } else if (!stick.GetRawButton(1)) actuatePressed = false; SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward); }
void Claw(double joy) { if(joy < 10) joy = 10; if(joy > 230) joy = 230; int location = EncClaw.Get(); double speed = PIDClaw(joy, location); //if(location < 15) speed *= .2; if(speed > .32) speed = .32; if(speed < -.32) speed = -.32; if(speed < .1 && speed > -.1) speed = 0; arm2->Set(speed,2); }
void StopWheels() { if (spinFastNow) { printf(">>> StopWheels\n"); Log(LOG_STOP, 0, 0); spinFastNow = false; #ifdef HAVE_TOP_WHEEL #ifdef HAVE_TOP_CAN1 jagStop(topWheel1); Log(LOG_MODE, 1, 0); #endif #ifdef HAVE_TOP_PWM1 topWheel1->Disable(); Log(LOG_MODE, 1, 0); #endif #ifdef HAVE_TOP_CAN2 jagStop(topWheel2); Log(LOG_MODE, 2, 0); #endif #endif #ifdef HAVE_BOTTOM_WHEEL #ifdef HAVE_BOTTOM_CAN1 jagStop(bottomWheel1); Log(LOG_MODE, 3, 0); #endif #ifdef HAVE_BOTTOM_PWM1 bottomWheel1->Disable(); Log(LOG_MODE, 3, 0); #endif #ifdef HAVE_BOTTOM_CAN2 jagStop(bottomWheel2); Log(LOG_MODE, 4, 0); #endif #endif topPID = bottomPID = false; printf("<<< StopWheels\n"); } }
/** * Run the closed loop position controller on the Jag. */ void OperatorControl() { printf("In OperatorControl\n"); GetWatchdog().SetEnabled(true); while (IsOperatorControl() && !IsDisabled()) { GetWatchdog().Feed(); // Set the desired setpoint speedJag.Set(stick.GetAxis(axis) * 150.0); UpdateDashboardStatus(); Wait(0.05); } }
void MecanumDrive::InitMotor( CANJaguar& motor ) { motor.ChangeControlMode( m_currControlMode ); if ( m_currControlMode == CANJaguar::kSpeed ) { motor.ConfigEncoderCodesPerRev(360); motor.ConfigMaxOutputVoltage(12.0); motor.ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); motor.SetPID(.7,.004,0); motor.SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder); } motor.EnableControl(); }
void MecanumDrive::CheckForRestartedMotor( CANJaguar& motor, const char * strDescription ) { if ( m_currControlMode != CANJaguar::kSpeed ) // kSpeed is the default { if ( motor.GetPowerCycled() ) { Wait(0.10); // Wait 100 ms InitMotor( motor ); char error[256]; sprintf(error, "\n\n>>>>%s %s", strDescription, "Jaguar Power Cycled - re-initializing"); printf(error); setErrorData(error, strlen(error), 100); } } }
void Arm(double joy) { int location = EncArm.Get(); /* if(location < 10 && joy < 0) joy = 0; if(location > 110 && joy > 0) joy = 0; arm1->Set(joy); arm1_sec->Set(joy); return; */ if(joy < -10) joy = -10; if(joy > 110) joy = 110; double speed = PIDArm(joy, location); if(speed > .5) speed = .5; if(speed < -.3) speed = -.3; if(speed < 0 && location < 10) speed = 0; if(speed > 0 && location > 110) speed = 0; speed = LowArm(speed); if(speed < .01 && speed > -.01) speed = 0; arm1->Set(speed,3); arm1_sec->Set(speed,3); }
void RawControl::resetArm() { arm->SetPID(-5, 0, 0); arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); arm->SetSafetyEnabled(false); arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer); arm->ConfigEncoderCodesPerRev(1); arm->EnableControl(0); }
void AutonomousPeriodic() { currentDistance = leftDriveEncoder->GetDistance(); shooterMotor->Set(-shooterMotorVolts); if (currentDistance >= goalDistance){ drive->setLeft(0); drive->setRight(0); loaderMotor->Set(Relay::kForward); } else { drive->setLeft(.49); drive->setRight(.5); } //log->info("LRD %f %f", // leftDriveEncoder->GetDistance(), // rightDriveEncoder->GetDistance()); //log->print(); }
void output (void) { REDRUM; if (IsAutonomous()) driverOut->PrintfLine(DriverStationLCD::kUser_Line1, "blaarag"); else if (IsOperatorControl()) { REDRUM; } outputCounter++; if (outputCounter % 30 == 0){ REDRUM; driverOut->PrintfLine(DriverStationLCD::kUser_Line2, "Top Shooter RPM: %f",(float)LTop.GetSpeed()); driverOut->PrintfLine(DriverStationLCD::kUser_Line3, "Bot Shooter RPM: %f",(float)LBot.GetSpeed()); // driverOut->PrintfLine(DriverStationLCD::kUser_Line6, "Pilot Z-Axis: %f",pilot.GetZ()); } if (outputCounter % 60 == 0){ REDRUM; driverOut->PrintfLine(DriverStationLCD::kUser_Line4, "Top CANJag Temp: %f Celcius",LTop.GetTemperature()*(9/5) + 32); driverOut->PrintfLine(DriverStationLCD::kUser_Line5, "Bot CANJag Temp: %f Celcius",LBot.GetTemperature()*(9/5) + 32); outputCounter = 1; } driverOut->UpdateLCD(); }//nom nom nom
void RawControl::resetJags() { { frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); frontRight->SetSafetyEnabled(false); frontRight->ConfigEncoderCodesPerRev(360); frontRight->EnableControl(0); } { frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); frontLeft->SetSafetyEnabled(false); frontLeft->ConfigEncoderCodesPerRev(360); frontLeft->EnableControl(0); } { backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); backLeft->SetSafetyEnabled(false); backLeft->ConfigEncoderCodesPerRev(360); backLeft->EnableControl(0); } { backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); backRight->SetSafetyEnabled(false); backRight->ConfigEncoderCodesPerRev(360); backRight->EnableControl(0); } frontLeft->ChangeControlMode(CANJaguar::kPercentVbus); frontRight->ChangeControlMode(CANJaguar::kPercentVbus); backLeft->ChangeControlMode(CANJaguar::kPercentVbus); backRight->ChangeControlMode(CANJaguar::kPercentVbus); frontLeft->ConfigFaultTime(0); backLeft->ConfigFaultTime(0); backRight->ConfigFaultTime(0); frontRight->ConfigFaultTime(0); frontLeft->EnableControl(0); frontRight->EnableControl(0); backLeft->EnableControl(0); backRight->EnableControl(0); }
void DoctaEight::OperatorControl(void) { REDRUM; LTop.SetSafetyEnabled(0); LTop.EnableControl(); LBot.SetSafetyEnabled(0); LBot.EnableControl(); while (IsOperatorControl()) { REDRUM; output(); if (pilot.GetRawButton(6)) { arm.Set (.3); } else if (pilot.GetRawButton(5)) { arm.Set (-.3); } else arm.Set (0); //Set Shooter state and reset RPMoffset if necessary if (copilot.GetRawButton(1)) //BUTTON A { REDRUM; if(ShooterState != 1) { REDRUM; ShooterState = 1; RPMoffset = 0; } } else if (copilot.GetRawButton(2)) //BUTTON B { REDRUM; if(ShooterState != 2) { REDRUM; ShooterState = 2; RPMoffset = 0; } } else if (copilot.GetRawButton(3)) //BUTTON X { REDRUM; if(ShooterState != 3) { REDRUM; ShooterState = 3; RPMoffset = 0; } } else if (copilot.GetRawButton(4)) //BUTTON Y { REDRUM; if(ShooterState != 4) { REDRUM; ShooterState = 4; RPMoffset = 0; } } //increment or decrement RPMoffset if(copilot.GetRawButton(5)) //BUTTON LB { REDRUM; FlagB5 = true; } else if(copilot.GetRawButton(6)) //BUTTON RB { REDRUM; FlagB6 = true; } if(FlagB5 == true && copilot.GetRawButton(5) == false) { REDRUM; RPMoffset -= 50; FlagB5 = false; } else if(FlagB6 && !copilot.GetRawButton(6)) { REDRUM; RPMoffset += 50; FlagB6 = false; } if (pilot.GetRawButton(1) && !cycle) { cycle = 1; negate*=-1; } else { cycle=0; } //prepare shooter/launcher ouput speed if(ShooterState == 1) { REDRUM; //BUTTON A LTopSpeed = MAX/12; LBotSpeed = RPMMid + RPMoffset; } else if(ShooterState == 2) { //BUTTON B REDRUM; LTopSpeed = MAX/12; LBotSpeed = RPMLow + RPMoffset; } else if(ShooterState == 3) { //BUTTON X REDRUM; LTopSpeed = MAX/12; LBotSpeed = RPMHigh + RPMoffset; } else if(ShooterState == 4) { //BUTTON Y REDRUM; LTopSpeed = 0; LBotSpeed = 0; } //set shooter launcher speed to CANJags LTop.Set(LTopSpeed); LBot.Set(-LBotSpeed); //move simple platform arm leftyrighty(-pilot.GetY(), -pilot.GetRawAxis(4)); intake.Set(-copilot.GetY()); } //stops encoders }
float setSpeed(float newMagnitude) { tarTheta = atan2(yVector, xVector); curTheta = -(posEncoder->GetVoltage() - FLOFFSET ) / 5 * 2 * PI; // Code Snippet diffTheta = tarTheta - curTheta; if (diffTheta > PI) { diffTheta -= 2*PI; } else if (diffTheta < -PI) { diffTheta += 2*PI; } if (diffTheta > PI/2) { diffTheta -= PI; mag = mag * -1; } else if (diffTheta < -PI/2) { diffTheta += PI; mag = mag * -1; } turnVel = diffTheta / (PI/2); if (0 < turnVel && turnVel < .25) { turnVel = .25; } if (0 > turnVel && turnVel > -.25) { turnVel = -.25; } if (fabs(diffTheta) < PI/45 ) { turnVel = 0; } if (((turnVel > 0 && prevTurnVel < 0) || (turnVel < 0&& prevTurnVel> 0)) && !changeSign) { changeSign = true; // moveTime = baneTimer.Get() + .1; **FIX BANETIMER } if (changeSign) { turnVel = 0; // if (moveTime < baneTimer.Get()) { changeSign = false; } } // /Code Snippet if (!(xVector == 0 && yVector == 0)) { turnWheel->Set(turnVel); moveWheel->Set(mag); } else { turnWheel->Set(0); moveWheel->Set(0); } }
//CHECK THIS OUT!! void RawControl::configJags() { //will need to be tuned on the new robot frontLeft->SetPID(3, .07, 0);//tested values are 1,.02,0 frontRight->SetPID(3, .09, 0); backLeft->SetPID(1, .013, 0); backRight->SetPID(1.2, .013, 0); arm->SetPID(ARM_P, -.02, 0); backLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); frontLeft->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); frontRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); backRight->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); arm->ConfigNeutralMode(CANJaguar::kNeutralMode_Brake); //shoot everything remotely safety related backLeft->SetSafetyEnabled(false); backRight->SetSafetyEnabled(false); frontLeft->SetSafetyEnabled(false); frontRight->SetSafetyEnabled(false); arm->SetSafetyEnabled(false); frontLeft->ConfigMaxOutputVoltage(13); frontRight->ConfigMaxOutputVoltage(13); backLeft->ConfigMaxOutputVoltage(13); backRight->ConfigMaxOutputVoltage(13); arm->ConfigMaxOutputVoltage(13); frontLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); frontRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); backRight->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); backLeft->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); arm->SetPositionReference(CANJaguar::kPosRef_Potentiometer); //not sure on these values either frontLeft->ConfigEncoderCodesPerRev(360); frontRight->ConfigEncoderCodesPerRev(360); backLeft->ConfigEncoderCodesPerRev(360); backRight->ConfigEncoderCodesPerRev(360); arm->ConfigPotentiometerTurns(1); frontLeft->ChangeControlMode(CANJaguar::kPercentVbus); frontRight->ChangeControlMode(CANJaguar::kPercentVbus); backLeft->ChangeControlMode(CANJaguar::kPercentVbus); backRight->ChangeControlMode(CANJaguar::kPercentVbus); frontLeft->EnableControl(0); frontRight->EnableControl(0); backLeft->EnableControl(0); backRight->EnableControl(0); frontLeft->ConfigFaultTime(0); backLeft->ConfigFaultTime(0); backRight->ConfigFaultTime(0); frontRight->ConfigFaultTime(0); arm->ConfigFaultTime(0); arm->EnableControl(0); arm->EnableControl(0); /* fl=new CANJaguar(2,CANJaguar::kSpeed); fr=new CANJaguar(3,CANJaguar::kSpeed); bl=new CANJaguar(4,CANJaguar::kSpeed); br=new CANJaguar(1,CANJaguar::kSpeed); fl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); fr->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); br->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); bl->SetSpeedReference(CANJaguar::kSpeedRef_Encoder); fl->ConfigEncoderCodesPerRev(1440); fr->ConfigEncoderCodesPerRev(1440); bl->ConfigEncoderCodesPerRev(1440); br->ConfigEncoderCodesPerRev(1440); */ }
int RawControl::zero() { arm->EnableControl(0); return 0; }
float RawControl::getArmPos() { return arm->GetPosition(); }