/** * Runs the motors with arcade steering. */ void OperatorControl(void) { HSLImage *Himage; Threshold targetThreshold(247, 255, 60, 140, 10, 50); BinaryImage *matchingPixels; vector<ParticleAnalysisReport> *pReport; //myRobot->SetSafetyEnabled(true); Saftey->SetEnabled(false); AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11"); mycam.WriteResolution(AxisCamera::kResolution_640x480); mycam.WriteCompression(20); mycam.WriteBrightness(25); Wait(3.0); dsLCD = DriverStationLCD::GetInstance(); dsLCD->Clear(); float X[2]; float Y[2]; float Z[2]; while(IsOperatorControl()) { X[1] = Stick1->GetX(); X[2] = Stick2->GetX(); Y[1] = Stick1->GetY(); Y[2] = Stick2->GetY(); Z[1] = Stick1->GetZ(); Z[2] = Stick2->GetZ(); Jaguar1->Set(Y[1]); Jaguar2->Set(Y[2]); Wait(0.005); if (mycam.IsFreshImage()) { Himage = mycam.GetImage(); matchingPixels = Himage->ThresholdHSL(targetThreshold); pReport = matchingPixels->GetOrderedParticleAnalysisReports(); for (unsigned int i = 0; i < pReport->size(); i++) { printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y); } delete Himage; delete matchingPixels; delete pReport; } } //myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick) //Wait(0.005); // wait for a motor update time }
void RobotDemo::interpolated_test_code() { float Test_Distance_Input = (((drive_stick_sec->GetZ() + 1) / 2.0) * 33.0) - 1; float Test_Distance_Output = lookup_distance_array(Test_Distance_Input); printf("input:%f output:%f\n", Test_Distance_Input, Test_Distance_Output); }
/** * Get the Z value of the current joystick. * This depends on the mapping of the joystick connected to the current port. * * @param port The USB port for this joystick. */ float GetZ(UINT32 port) { Joystick *stick = getJoystick(port); if (stick == NULL) return 0; return stick->GetZ(); }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { bool collisionDetected = false; double curr_world_linear_accel_x = ahrs->GetWorldLinearAccelX(); double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x; last_world_linear_accel_x = curr_world_linear_accel_x; double curr_world_linear_accel_y = ahrs->GetWorldLinearAccelY(); double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y; last_world_linear_accel_y = curr_world_linear_accel_y; if ( ( fabs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G ) || ( fabs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G) ) { collisionDetected = true; } SmartDashboard::PutBoolean( "CollisionDetected", collisionDetected); try { /* Use the joystick X axis for lateral movement, */ /* Y axis for forward movement, and Z axis for rotation. */ /* Use navX MXP yaw angle to define Field-centric transform */ robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ(),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 } }
/** * @brief Sets a cached joystick value. * @param joy_id Which joystick to set the cached value for. * @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons. */ void Proxy::SetJoystick(int joy_id, Joystick & stick) { wpi_assert(joy_id < NUMBER_OF_JOYSTICKS+1 && joy_id >= 0); char tmp[32]; sprintf(tmp, "Joy%d", joy_id); string name = tmp; if(!disableAxes[joy_id-1]) { set(name + 'X', stick.GetX()); set(name + 'Y', stick.GetY()); set(name + 'Z', stick.GetZ()); set(name + 'R', stick.GetTwist()); set(name + 'T', stick.GetThrottle()); for(int AxisId=1; AxisId<=6; AxisId++) { sprintf(tmp, "%sA%d", name.c_str(), AxisId); set(tmp, stick.GetRawAxis(AxisId)); } } else { if(!stick.GetRawButton(disableAxes[joy_id-1])) { set(name + 'X', stick.GetX()); set(name + 'Y', stick.GetY()); set(name + 'Z', stick.GetZ()); set(name + 'R', stick.GetTwist()); set(name + 'T', stick.GetThrottle()); for(int AxisId=1; AxisId<=6; AxisId++) { sprintf(tmp, "%sA%d", name.c_str(), AxisId); set(tmp, stick.GetRawAxis(AxisId)); } } } if(!disableButtons[joy_id-1]) { for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) { sprintf(tmp, "%sB%d", name.c_str(), i); set(tmp,stick.GetRawButton(i)); } set(name + "BT", stick.GetTrigger()); } else { if(!stick.GetRawButton(disableButtons[joy_id-1])) { for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) { sprintf(tmp, "%sB%d", name.c_str(), i); set(tmp,stick.GetRawButton(i)); } set(name + "BT", stick.GetTrigger()); } } }
void shootTask(){ while(true){ if(true){ if(!controller->GetRawButton(1)||controller->GetRawButton(3)){ if(controller->GetRawButton(5)||controller->GetRawButton(6)){ if(controller->GetRawButton(5)){ mySword->Intake(true); } else if(controller->GetRawButton(6)){ mySword->Release(); } } else{ mySword->Intake(false); mySword->StopIntake(); mySword->StopIndexer(); } } else if(controller->GetRawButton(1)){ mySword->Shoot(); } } if(true){ if(controller->GetZ() > 0.1){ mySword->LiftIntake(); } else if(controller->GetZ() < -0.1){ mySword->LowerIntake(); } } if(true){ if(controller->GetRawButton(3)){ mySword->Prime(true); }else{ mySword->Prime(false); } } Wait(0.005); } }
/** * Drive based upon joystick inputs, and automatically control * motors if the robot begins tipping. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { double xAxisRate = stick.GetX(); double yAxisRate = stick.GetY(); double pitchAngleDegrees = ahrs->GetPitch(); double rollAngleDegrees = ahrs->GetRoll(); if ( !autoBalanceXMode && (fabs(pitchAngleDegrees) >= fabs(kOffBalanceThresholdDegrees))) { autoBalanceXMode = true; } else if ( autoBalanceXMode && (fabs(pitchAngleDegrees) <= fabs(kOnBalanceThresholdDegrees))) { autoBalanceXMode = false; } if ( !autoBalanceYMode && (fabs(pitchAngleDegrees) >= fabs(kOffBalanceThresholdDegrees))) { autoBalanceYMode = true; } else if ( autoBalanceYMode && (fabs(pitchAngleDegrees) <= fabs(kOnBalanceThresholdDegrees))) { autoBalanceYMode = false; } // Control drive system automatically, // driving in reverse direction of pitch/roll angle, // with a magnitude based upon the angle if ( autoBalanceXMode ) { double pitchAngleRadians = pitchAngleDegrees * (M_PI / 180.0); xAxisRate = sin(pitchAngleRadians) * -1; } if ( autoBalanceYMode ) { double rollAngleRadians = rollAngleDegrees * (M_PI / 180.0); yAxisRate = sin(rollAngleRadians) * -1; } try { // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. robotDrive.MecanumDrive_Cartesian(xAxisRate, yAxisRate,stick.GetZ()); } catch (std::exception ex ) { std::string err_string = "Drive system error: "; err_string += ex.what(); DriverStation::ReportError(err_string.c_str()); } Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
void RobotDemo::z_axis_control() { //shooter_motor_front->Set((-operator_stick->GetZ() + 1.0) / 2.0);//Shooter motors must always be set to positive values //shooter_motor_back->Set((-operator_stick->GetZ() + 1.0) / 2.0); if (!constant_desired_RPS) { desired_RPS_control = ((-operator_stick->GetZ() + 1) / 2) * 75; RPS_control_code(desired_RPS_control); } }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. // This sample does not use field-oriented drive, so the gyro input is set to zero. robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ()); Wait(0.005); // wait 5ms to avoid hogging CPU cycles } }
/** * @brief Sets a cached joystick value. * @param joy_id Which joystick to set the cached value for. * @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons. */ void Proxy166::SetJoystick(int joy_id, Joystick & stick) { wpi_assert(joy_id < NUMBER_OF_JOYSTICKS && joy_id >= 0); //semTake(JoystickLocks[joy_id], WAIT_FOREVER); Joysticks[joy_id].X = stick.GetX(); Joysticks[joy_id].Y = stick.GetY(); Joysticks[joy_id].Z = stick.GetZ(); Joysticks[joy_id].throttle = stick.GetThrottle(); for(unsigned i=0;i<NUMBER_OF_JOY_BUTTONS;i++) { Joysticks[joy_id].button[i] = stick.GetRawButton(i); } //semGive(JoystickLocks[joy_id]); }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { log->Info("TELEOP START"); // myRobot.SetSafetyEnabled(true); while (IsOperatorControl()) { double distance = dist.GetVoltage() / (5.0 / 512.0); SmartDashboard::Log(distance, "Rangefinder distance"); SmartDashboard::Log(dist.GetVoltage(), "Rangefinder voltage"); Sol1.Set(stick1.GetRawButton(1)); Sol2.Set(stick1.GetRawButton(2)); Sol3.Set(stick1.GetRawButton(3)); Sol4.Set(stick1.GetRawButton(4)); Sol5.Set(stick1.GetRawButton(5)); if (stick1.GetRawButton(6)) { if (!lastButton) log->Shot(stick1.GetZ(), stick2.GetZ()); lastButton = true; } else { lastButton = false; } SmartDashboard::Log(((stick1.GetZ() + 1) / 2) * 2500, "Distance"); SmartDashboard::Log(((stick2.GetZ() + 1) / 2) * 5.0, "Compression"); SmartDashboard::Log(LookUp(stick1.GetZ() * 2500, /* stick2.GetZ() * 5.0 */ dist.GetVoltage()), "Shooter Speed"); // myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick) // lJagA.Set(stick1.GetY()); // lJagB.Set(stick1.GetY()); // rJagA.Set(stick2.GetY()); // rJagB.Set(stick2.GetY()); Wait(0.010); // wait for a motor update time } }
void RobotDemo::constant_RPS_code() { if (operator_stick->GetRawButton(front_position_button)) { desired_RPS_control = front_position_RPS; } else { if (operator_stick->GetRawButton(back_position_button_1)) { desired_RPS_control = back_position_RPS_1; } else if (operator_stick->GetRawButton(back_position_button_2)) { desired_RPS_control = back_position_RPS_2; } else if (operator_stick->GetRawButton(back_position_button_3)) { desired_RPS_control = back_position_RPS_3; } else if (operator_stick->GetRawButton(back_position_button_4)) { desired_RPS_control = back_position_RPS_4; } else if (operator_stick->GetRawButton(back_position_button_5)) { desired_RPS_control = back_position_RPS_5; } else { if (operator_stick->GetRawButton(dumper_button_A) || operator_stick->GetRawButton(dumper_button_B)) { desired_RPS_control = dumper_RPS; } else { desired_RPS_control = ((-operator_stick->GetZ() + 1) / 2) * 75; } } } RPS_control_code(desired_RPS_control); }
/** * Runs the motors with Mecanum drive. */ void OperatorControl() { robotDrive.SetSafetyEnabled(false); while (IsOperatorControl() && IsEnabled()) { bool motionDetected = ahrs->IsMoving(); SmartDashboard::PutBoolean("MotionDetected", motionDetected); try { /* Use the joystick X axis for lateral movement, */ /* Y axis for forward movement, and Z axis for rotation. */ /* Use navX MXP yaw angle to define Field-centric transform */ robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), stick.GetZ(),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 TeleopPeriodic() { if(stick.GetRawButton(3)) { sm2kl.Set(1.0); sm2kr.Set(1.0); } if(rSwitch.Get()) { sm2kr.Set(0.0); sm2kl.Set(0.0); SmartDashboard::PutNumber("Tests",675); } if(stick.GetRawButton(5)) { sm2kl.Set(-1.0); sm2kr.Set(-1.0); SmartDashboard::PutNumber("Tests",675); } if((lEnc.GetDistance() > 1080)||(rEnc.GetDistance() > 1080)) { sm2kl.Set(0.0); sm2kr.Set(0.0); } if(stick.GetRawButton(10)) { actintake.Set(1.0); } else if(stick.GetRawButton(9)) { actintake.Set(-1.0); } else { actintake.Set(0.0); } xvalue = -stick.GetZ()*.5; yvalue = -stick.GetY(); if(stick.GetRawButton(7)&&!toggle) { latch=!latch; toggle=true; } if(!stick.GetRawButton(7)&&toggle) { toggle=false; } if(latch) { yvalue=-yvalue; } if(stick.GetRawButton(1)) { launcher.Set(1.0); } if(stick.GetRawButton(11)) { intake.Set(1.0); } myRobot.ArcadeDrive(yvalue,xvalue); SmartDashboard::PutNumber("rSwitch",rSwitch.Get()); SmartDashboard::PutBoolean("latch",latch); SmartDashboard::PutNumber("Button",stick.GetRawButton(5)); //myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick) }
float ScaleZ( Joystick& stick) { // CONSTANT^-1 is step value (now 1/500) return floorf( 500.f * ( 1.f - stick.GetZ() ) / 2.f ) / 500.f; }
/** * Runs the motors with arcade steering. */ void OperatorControl(void) { myRobot->SetSafetyEnabled(true); while (IsOperatorControl()) { bool setLimit; double cimValue1= -scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1, also invert the cim, since we want it to rotate coutnerclockwise/clockwise double cimValue2= scaleThrottle(-(stick->GetZ())); //Set desired speed from the Throttle, assuming from -1 to 1 //For shooter /*if (stick.GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick myRobot.ArcadeDrive((stick.GetY()), (stick.GetX()), false); // inverted drive control } else { //front of the robot is moved forward by pushing forward on the joystick myRobot.ArcadeDrive(-(stick.GetY()), (stick.GetX()), false); // normal drive control } */ //For manual button speed control, this sets the speed if (stick->GetRawButton(4) == true) { cim1->Set(cimValue1); //use the value from the throttle to set cim speed cim2->Set(cimValue2);//Get speed from throttle, and then scale it setLimit = true; //Open Ball Stopper } else { cim1->Set(0.0); cim2->Set(0.0); setLimit = false; //Close Ball Stopper } //For precisebelt pickup if (stick->GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick if (setLimit == true) { JagBelt->ArcadeDrive(0.1, (stick->GetX()), false); // inverted drive control } else { JagBelt->ArcadeDrive((stick->GetY()), (stick->GetX()), false); // inverted drive control } } else { //front of the robot is moved forward by pushing forward on the joystick if (setLimit == true) { JagBelt->ArcadeDrive(-0.1, (stick->GetX()), false); // inverted drive control } else { JagBelt->ArcadeDrive(-(stick->GetY()), (stick->GetX()), false); // inverted drive control } } //For normal belt pickup /*if (stick->GetRawButton(6) == true) { JagBelt->Drive(1.0, 0); //opens gripper } else { JagBelt->Drive(0.0, 0); //closes gripper } */ //For drive if (rightstick->GetRawButton(1) == true) { //back of the robot is moved forward by pushing forward on the joystick if (rightstick->GetRawButton(10) == true) { precisionMode= true; } else { precisionMode= false; } myRobot->ArcadeDrive((rightstick->GetY()), (rightstick->GetX()), precisionMode); // inverted drive control } else { if (rightstick->GetRawButton(10) == true) { precisionMode= true; } else { precisionMode= false; } //front of the robot is moved forward by pushing forward on the joystick myRobot->ArcadeDrive(-(rightstick->GetY()), (rightstick->GetX()), precisionMode); // normal drive control } /*if (stick->GetRawButton(8) == true) { cim1->Set(-0.37); //run cim 1 at 50% speed counterclockwise?? cim2->Set(0.37); // run cim at 50% speed clockwise check = true; //indicate if motors are running } else if (check == true){ // if motors are running a Wait(2.0); belt->Set(1); // run the belt Wait(2.0); // one sec delay belt->Set(0.0); // turn belt off check = false; // put new check } else if (check == false){ //if false // 2 sec delay to wait for the first ball to shoot cim1->Set(0.0); //stop cims cim2->Set(0.0); } else { // Stop everything cim1->Set(0.0); //stop cims cim2->Set(0.0); belt->Set(0.0); } */ //Code for using window motor if (rightstick->GetRawButton(4)) { window->Set(Relay::kOn); window->Set(Relay::kForward); // tell window motor to go forward } else if (rightstick->GetRawButton(5) == true){ window->Set(Relay::kOn); window->Set(Relay::kReverse); //tell window motor to go backward } else { //Wait(1.0); window->Set(Relay::kOff); //turn it off, if the relays aint being used } //Code for Banebot Motor for stopping ballz if (stick->GetRawButton(2) == true) { // press button TWO to close pickStop->Set(-0.5); //closes ball stopper Wait(1); pickStop->Set(0.0); } else if (stick->GetRawButton(3) == true){ //press button three to open pickStop->Set(0.5); //opens ball stopper Wait(1.2); // too slow, so needs more time pickStop->Set(0.0); } else if (stick->GetRawButton(5)== true){ //press 5 to stop imediately, useful for adjusting angles... //Wait(1.0); pickStop->Set(0.0); } //Code for ... servoooo if (stick->GetRawButton(10) == true) { //press 10 on the left stick... bar->SetAngle(60); // set the angles to 60...clockwise? bar2->SetAngle(60); } else if (stick->GetRawButton(11) == true) // press 11 on the left stick { bar->SetAngle(-60); //set the angles to -60...counterclockwise? bar2->SetAngle(-60); } // Initialize functions... // RelayServo(); //PreciseBelt(); //UltrasonicRange(); // Accelerometer(); //dash->GetPacketNumber(); // send data back to dashboard dash->GetPacketNumber(); //not sure why this is here 0_0 //int limitValue= limitSwitch->Get() ? 1 : 0; // retrieve 1 and 0 only.../ look for 0 and 1 float servoAngle = bar->GetAngle(); //dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Limit State: %d", limitValue); //send data back to driver station... // dsLCD->Printf(DriverStationLCD::kUser_Line2, 2, "Servo Angle: %f", servoAngle); //send data back to driver station... float gyroVal = gyro -> GetVoltage();//Gets voltage from gyro float ultraVal = rangeFinder -> GetVoltage(); //Get voltage from ultrasonic sensor float tempVal = Temperature -> GetVoltage();//Gets temperature //Do the math to convert data received from the ultrasonic volts->miliVolts->milivolts per inch->inches float Vm = (ultraVal*1000); float Ri = (Vm/9.765625); // Print data back to dashboard dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Ultrasonic Range: %f",Ri); dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Gyro: %f", gyroVal); dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Temperature: %f", tempVal); dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Cim1 Speed: %f%%", (cimValue1*100)); //display speed that the mototrs are reunning at different percentages... dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Cim2 Speed: %f%%", (cimValue2*100)); //Update dashboard dsLCD->UpdateLCD(); } }
/** * Runs the motors with Mecanum drive. */ void OperatorControl()//teleop code { robotDrive.SetSafetyEnabled(false); gyro.Reset(); grabEncoder.Reset(); timer.Start(); timer.Reset(); double liftHeight = 0; //variable for lifting thread int liftHeightBoxes = 0; //another variable for lifting thread int liftStep = 0; //height of step in inches int liftRamp = 0; //height of ramp in inches double grabPower; bool backOut; uint8_t toSend[10];//array of bytes to send over I2C uint8_t toReceive[10];//array of bytes to receive over I2C uint8_t numToSend = 1;//number of bytes to send uint8_t numToReceive = 0;//number of bytes to receive toSend[0] = 1;//set the byte to send to 1 i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C bool isGrabbing = false;//whether or not grabbing thread is running bool isLifting = false;//whether or not lifting thread is running bool isBraking = false;//whether or not braking thread is running float driveX = 0; float driveY = 0; float driveZ = 0; float driveGyro = 0; bool liftLastState = false; bool liftState = false; //button pressed double liftLastTime = 0; double liftTime = 0; bool liftRan = true; Timer switchTimer; Timer grabTimer; switchTimer.Start(); grabTimer.Start(); while (IsOperatorControl() && IsEnabled()) { // Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. // This sample does not use field-oriented drive, so the gyro input is set to zero. toSend[0] = 1; numToSend = 1; driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code driveY = driveStick.GetRawAxis(Constants::driveYAxis); driveZ = driveStick.GetRawAxis(Constants::driveZAxis); driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset; if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X toSend[0] = 6; numToSend = 1; if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) { driveY = 0; driveZ = 0; } else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y driveX = 0; driveZ = 0; } else {//if Z is greater than X and Y, then it will only go in the direction of Z driveX = 0; driveY = 0; } } if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function toSend[0] = 7; driveZ = 0;//Stops Z while Z lock is pressed } if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field driveGyro = 0;//gyro stops while field lock is enabled } driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree); driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree); driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree); robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) { pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber if(grabTimer.Get() < 1) { toSend[0] = 6; } } else { pickup.setGrabber(0); grabTimer.Reset(); toSend[0] = 6; } if (Constants::grabLiftInverted) { pickup.setLifter(-Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter } else { pickup.setLifter(Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter } SmartDashboard::PutNumber("Lift Power", Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); SmartDashboard::PutBoolean("Is Lifting", isLifting); if (Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree) != 0 || isLifting) { //if the robot is lifting isBraking = false; //stop braking thread SmartDashboard::PutBoolean("Braking", false); } else if(!isBraking) { isBraking = true; //run braking thread pickup.lifterBrake(isBraking);//brake the pickup } if (grabStick.GetRawButton(Constants::liftFloorButton)) { liftHeight = 0; pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread liftRan = true; } liftTime = timer.Get(); liftState = grabStick.GetRawButton(Constants::liftButton); if (liftState) { //if button is pressed if (!liftLastState) { if (liftTime - liftLastTime < Constants::liftMaxTime) { if (liftHeightBoxes < Constants::liftMaxHeightBoxes) { liftHeightBoxes++; //adds 1 to liftHeightBoxes } } else { liftHeightBoxes = 1; liftRamp = 0; liftStep = 0; } } liftLastTime = liftTime; liftLastState = true; liftRan = false; } else if (grabStick.GetRawButton(Constants::liftRampButton)) { if (liftTime - liftLastTime > Constants::liftMaxTime) { liftHeight = 0; liftStep = 0; } liftRamp = 1; //prepares to go up ramp liftLastTime = liftTime; liftRan = false; } else if (grabStick.GetRawButton(Constants::liftStepButton)) { if (liftTime - liftLastTime > Constants::liftMaxTime) { liftHeight = 0; liftRamp = 0; } liftStep = 1; //prepares robot for step liftLastTime = liftTime; liftRan = false; } else { if (liftTime - liftLastTime > Constants::liftMaxTime && !liftRan) { liftHeight = liftHeightBoxes * Constants::liftBoxHeight + liftRamp * Constants::liftRampHeight + liftStep * Constants::liftStepHeight; //sets liftHeight if (liftHeightBoxes > 0) { liftHeight -= Constants::liftBoxLip; } pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread liftRan = true; } liftLastState = false; } if (grabStick.GetRawButton(Constants::grabToteButton)) {//if grab button is pressed grabPower = Constants::grabToteCurrent; backOut = true; if (!isGrabbing) { pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread } } else if (grabStick.GetRawButton(Constants::grabBinButton)) {//if grab button is pressed grabPower = Constants::grabBinCurrent; backOut = false; if (!isGrabbing) { pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread } } else if (grabStick.GetRawButton(Constants::grabChuteButton)) {//if grab button is presset SmartDashboard::PutBoolean("Breakpoint -2", false); SmartDashboard::PutBoolean("Breakpoint -1", false); SmartDashboard::PutBoolean("Breakpoint 0", false); SmartDashboard::PutBoolean("Breakpoint 1", false); SmartDashboard::PutBoolean("Breakpoint 2", false); SmartDashboard::PutBoolean("Breakpoint 3", false); SmartDashboard::PutBoolean("Breakpoint 4", false); //Wait(.5); if (!isGrabbing) { //pickup.grabberChute(isGrabbing, grabStick);//start grabber thread } } //determines what the LED's look like based on what the Robot is doing if (isGrabbing) { toSend[0] = 5; numToSend = 1; } if (isLifting) {//if the grabbing thread is running if (Constants::encoderToDistance(liftEncoder.Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < liftHeight) { toSend[0] = 3; } else { toSend[0] = 4; } numToSend = 1;//sends 1 byte to I2C } if(!grabOuterLimit.Get()) { //tells if outer limit is hit with lights if(switchTimer.Get() < 1) { toSend[0] = 6; } } else { switchTimer.Reset(); } if (driveStick.GetRawButton(Constants::sneakyMoveButton)) { toSend[0] = 0; numToSend = 1; } float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor float rotations = (float) liftEncoder.Get(); // rotations on encoder SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash SmartDashboard::PutNumber("Current", pdp.GetCurrent(Constants::grabPdpChannel)); SmartDashboard::PutNumber("LED Current", pdp.GetCurrent(Constants::ledPdpChannel)); SmartDashboard::PutNumber("Lift Encoder", rotations); SmartDashboard::PutNumber("Lift Height", liftHeight); SmartDashboard::PutNumber("Grab Encoder", grabEncoder.Get()); SmartDashboard::PutBoolean("Grab Inner", grabInnerLimit.Get()); SmartDashboard::PutBoolean("Grab Outer", grabOuterLimit.Get()); SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin)); SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin)); SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin)); SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin)); SmartDashboard::PutNumber("Throttle", grabStick.GetZ()); i2c.Transaction(toSend, 1, toReceive, 0);//send and receive information from arduino over I2C Wait(0.005); // wait 5ms to avoid hogging CPU cycles } //end of teleop isBraking = false; toSend[0] = 0; i2c.Transaction(toSend, numToSend, toReceive, numToReceive); }
void Autonomous() { Timer timer; float power = 0; bool isLifting = false; bool isGrabbing = false; double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip; double grabPower = Constants::grabAutoCurrent; bool backOut; uint8_t toSend[1];//array of bytes to send over I2C uint8_t toReceive[0];//array of bytes to receive over I2C uint8_t numToSend = 1;//number of bytes to send uint8_t numToReceive = 0;//number of bytes to receive toSend[0] = 2;//set the byte to send to 1 i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C bool isSettingUp = true; //pickup.setGrabber(-1); //open grabber all the way pickup.setLifter(0.8); while (isSettingUp && IsEnabled() && IsAutonomous()) { isSettingUp = false; /*if (grabOuterLimit.Get() == false) { pickup.setGrabber(0); //open until limit } else { isSettingUp = true; }*/ if (liftLowerLimit.Get()) { pickup.setLifter(0); //down till bottom } else { isSettingUp = true; } } gyro.Reset(); liftEncoder.Reset(); grabEncoder.Reset(); if (grabStick.GetZ() > .8) { timer.Reset(); timer.Start(); while (timer.Get() < 1) { robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back if(power>-.4){ power-=0.005; Wait(.005); } } robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle()); // STOP!!! timer.Stop(); timer.Reset(); Wait(1); } power = 0; while (isLifting && IsEnabled() && IsAutonomous()) { Wait(.005); } backOut = Constants::autoBackOut; pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick); Wait(.005); while (isGrabbing && IsEnabled() && IsAutonomous()) { Wait(.005); } liftHeight = 3*Constants::liftBoxHeight; Wait(.005); pickup.lifterPosition(liftHeight, isLifting, grabStick); Wait(.005); while (isLifting && IsEnabled() && IsAutonomous()) { Wait(.005); } while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous()); // while the nearest object is closer than 2 feet timer.Start(); while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet if (power < .45) { //ramp up the power slowly power += .00375; } robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle()); // drive back float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12; // distance from ultrasonic sensor SmartDashboard::PutNumber("Distance", distance); // write stuff to smart dash SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin)); SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin)); SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin)); SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin)); SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle()); SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches); Wait(.005); } timer.Reset(); while(timer.Get() < Constants::autoBrakeTime && IsEnabled() && IsAutonomous()) { // while the nearest object is further than 12 feet robotDrive.MecanumDrive_Cartesian(0,Constants::autoBrakePower,0); ///Brake } float turn = 0; while (fabs(turn) < 85 && IsEnabled() && IsAutonomous()) { //turn 90(ish) degrees robotDrive.MecanumDrive_Cartesian(0,0,.1); turn = gyro.GetAngle(); if (turn > 180) { turn -= 360; } } robotDrive.MecanumDrive_Cartesian(0,0,0); ///STOP!!! timer.Stop(); toSend[0] = 8; i2c.Transaction(toSend, numToSend, toReceive, numToReceive); while(IsAutonomous() && IsEnabled()); toSend[0] = 0; i2c.Transaction(toSend, numToSend, toReceive, numToReceive); }
void OperatorControl(void) { JankyTurret turret(7,11,10); JankyTargeting targ(&turret); JankyShooter shoot(SHOOTER_JAGUAR_CHANNEL,SHOOTER_ENCODER_A,SHOOTER_ENCODER_B); int rpmForShooter=0; while (IsOperatorControl()) { //GetImage() //hsl.Save // myRobot.ArcadeDrive(stick); shoot.GetCurrentRPM(); if (button.Get()==true) { LEDRelay.Set(Relay::kForward); if (button_H.Get()==true) { targ.SetLMHTarget(BOGEY_H); SmartDashboard::PutString("Targeting","High Button Pressed"); } if (button_M.Get()==true) { targ.SetLMHTarget(BOGEY_M); SmartDashboard::PutString("Targeting","Medium Button Pressed"); } if (button_L.Get()==true) { targ.SetLMHTarget(BOGEY_L); SmartDashboard::PutString("Targeting","Low Button Pressed"); } if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true) { if (targ.ProcessOneImage()) { targ.ChooseBogey(); targ.MoveTurret(); rpmForShooter = targ.GetCalculatedRPM(); shoot.setTargetRPM(rpmForShooter); targ.InteractivePIDSetup(); } } targ.StopPID(); shoot.setTargetRPM(0); } else { float lazysusan =stick.GetZ(); turret.Set(lazysusan); LEDRelay.Set(Relay::kOff); // Set shooter speed to zero. } // float desired=abs(stick.GetY() *1000) + 100; // smarty->PutInt("Desired RPM1", (int)desired); // shoot.setTargetRPM((int)desired); Wait(0.05); } // After teleop runs, save preferences again. Preferences::GetInstance()->Save(); }
/**************************************** * Runs the motors with arcade steering.* ****************************************/ void OperatorControl(void) { //TODO put in servo for lower camera--look in WPI to set // Watchdog baddog; // baddog.Feed(); myRobot.SetSafetyEnabled(true); //SL Earth.Start(); // turns on Earth // SmartDashboard *smarty = SmartDashboard::GetInstance(); //DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory) //char debugout [100]; compressor.Start(); gyro.Reset(); // resets gyro angle int rpmForShooter; while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop" { // baddog.Feed(); //myRobot.SetSafetyEnabled(true); //myRobot.SetExpiration(0.1); float leftYaxis = driver.GetY(); float rightYaxis = driver.GetTwist();//RawAxis(5); myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1 float random = gamecomponent.GetY(); float lazysusan = gamecomponent.GetZ(); //bool elevator = Frodo.Get(); float angle = gyro.GetAngle(); bool balance = Smeagol.Get(); SmartDashboard::PutNumber("Gyro Value",angle); int NumFail = -1; //bool light = Pippin.Get(); //SL float speed = Earth.GetRate(); //float number = shooter.Get(); //bool highspeed = button1.Get() //bool mediumspeed = button2.Get(); //bool slowspeed = button3.Get(); bool finder = autotarget.Get(); //bool targetandspin = autodistanceandspin.Get(); SmartDashboard::PutString("Targeting Activation",""); //dslcd->Clear(); //sprintf(debugout,"Number=%f",angle); //dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout); //SL sprintf(debugout,"Number=%f",speed); //SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout); //sprintf(debugout,"Number=%f",number); //dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout); //sprintf(debugout,"Finder=%u",finder); //dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout); //dslcd->UpdateLCD(); // update the Driver Station with the information in the code // sprintf(debugout,"Number=%u",maxi); // dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout) bool basketballpusher = julesverne.Get(); bool bridgetipper = joystickbutton.Get(); if (bridgetipper) // if joystick button 7 is pressed (is true) { solenoid.Set(true); // then the first solenoid is on } else { //Wait(0.5); // and then the first solenoid waits for 0.5 seconds solenoid.Set(false); //and then the first solenoid turns off } if (basketballpusher) // if joystick button 6 is pressed (is true) { shepard.Set(true); // then shepard is on the run //Wait(0.5); // and shepard waits for 0.5 seconds } else { shepard.Set(false); // and then shepard turns off } //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2 //} //cheetah.Set(0.3*lazysusan); // smarty->PutDouble("pre-elevator",lynx.Get()); lynx.Set(random); // smarty->PutDouble("elevator",lynx.Get()); // smarty->PutDouble("joystick elevator",random); if (balance) // this is the start of the balancing code { angle = gyro.GetAngle(); myRobot.Drive(-0.03*angle, 0.0); Wait(0.005); } /*if (light) //button 5 turns light on oand off on game controller flashring.Set(Relay::kForward); else flashring.Set(Relay::kOff); */ if (finder) { flashring.Set(Relay::kForward); if (button_H.Get()==true) { targeting.SetLMHTarget(BOGEY_H); SmartDashboard::PutString("Targeting","High Button Pressed"); } if (button_M.Get()==true) { targeting.SetLMHTarget(BOGEY_M); SmartDashboard::PutString("Targeting","Medium Button Pressed"); } if (button_L.Get()==true) { targeting.SetLMHTarget(BOGEY_L); SmartDashboard::PutString("Targeting","Low Button Pressed"); } if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true) { if (targeting.ProcessOneImage()) { NumFail = 0; SmartDashboard::PutString("Targeting Activation","YES"); targeting.ChooseBogey(); targeting.MoveTurret(); #ifdef USE_HARDWIRED_RPM shooter.setTargetRPM(HARDWIRED_RPM); #else rpmForShooter = targeting.GetCalculatedRPM(); shooter.setTargetRPM(rpmForShooter); #endif targeting.InteractivePIDSetup(); } else { NumFail++; if (NumFail > 10) targeting.StopPID(); } SmartDashboard::PutNumber("Numfail", NumFail); shooter.setTargetRPM(rpmForShooter); } else { SmartDashboard::PutString("Targeting Activation","NO"); shooter.setTargetRPM(0); targeting.StopPID(); } } else { flashring.Set(Relay::kOff); targeting.StopPID(); turret.Set(lazysusan); // the lazy susan would turn right & left based on how far the person moves the right joystick#2 side to side //targeting.StopPID(); //if (elevator) //shooter would move at full speed if button is pressed //TODO Change RPM values //TODO Disable calculation of RPM values SmartDashboard::PutNumber("CurrentRPM",shooter.GetCurrentRPM()); if (button_H.Get() == true) shooter.setTargetRPM((int)2100); //From front of free throw line, should hit the backboard and go in //used to be 2700 RPMs else if (button_M.Get() == true) shooter.setTargetRPM((int)1900); //From front of free throw line, should go in the net--can shoot the next ball on the overshoot? //Used to be 2250 RPMs else if (button_L.Get() == true) shooter.setTargetRPM((int)1350); //From fender, should hit the backboard //Used to be 2000 RPMs //shooter.Set(0.5); else shooter.setTargetRPM(0); // else if (mediumspeed) //shooter.setTargetRPM((int)0); //else if (slowspeed) //shooter.setTargetRPM((int)0); /*if (targetandspin) //code for autotargeting and speed will go here { shooter.setTargetRPM((int)1800); } else {*/ //} myRobot.TankDrive(leftYaxis,rightYaxis); } //Wait(0.005); } }
void TeleopPeriodic() { if(tick==10) if (ds->IsSysBrownedOut()) { ds->ReportError("[ERROR] BROWNOUT DETECTED!!"); } if(tick == 15) if (!ds->IsNewControlData()) { ds->ReportError( "[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!"); } if(tick==20) if (!ds->IsDSAttached()) { ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!"); } if (stick.GetRawButton(10)) zeroSanics(); if (stick.GetRawButton(8)) { leftIRZero = 0; rightIRZero = 0; } tick++; if (liftStick.GetRawButton(2)) { double canScale = liftStick.GetRawAxis(2); canScale += 1; canScale = 2 - canScale; canScale /= 2; canGrabber.SetSpeed(canScale); } else if (liftStick.GetRawButton(3)) { double canScale = liftStick.GetRawAxis(2); canScale += 1; canScale = 2 - canScale; canScale /= 2; canGrabber.SetSpeed(-canScale); } else canGrabber.SetSpeed(0); double speed; //Calculate scalar to use for POV/Adjusted drive double scale = stick.GetRawAxis(3); scale += 1; scale = 2 - scale; scale /= 2; //Use pov/hat switch for movement if enabled if (stick.GetRawButton(1) && stick.GetRawButton(2)) { AutomaticLineup(); } else if (stick.GetRawButton(1)) { double leftVolts = leftIR.GetAverageVoltage() - leftIRZero; double rightVolts = rightIR.GetAverageVoltage() - leftIRZero; if (rightVolts + VOLTAGE_TOLERANCE > leftVolts && rightVolts - VOLTAGE_TOLERANCE < leftVolts) { robotDrive.MecanumDrive_Cartesian(0, 0, 0); } else if (rightVolts > leftVolts) robotDrive.MecanumDrive_Cartesian(0, 0, 0.2); else if (leftVolts > rightVolts) robotDrive.MecanumDrive_Cartesian(0, 0, -0.2); } else if (stick.GetRawButton(6)) { //Rotate robotDrive.MecanumDrive_Polar(0, 0, scale); } else if (stick.GetRawButton(5)) { //Rotate robotDrive.MecanumDrive_Polar(0, 0, -scale); } else if (stick.GetPOV(0) != -1) { //If POV moved, move polar (getPOV returns an angle in degrees) robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0); } else if (stick.GetRawButton(2)) { //Drive with scalar robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale, stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale); } else { //Drive normally robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(), stick.GetZ()); } speed = -liftStick.GetY(); //bool canGoUp = maxUp.Get(); bool canGoUp = true; //bool canGoDown = maxDown.Get(); bool canGoDown = true; //If at a limit switch and moving in that direction, stop if (speed > 0 && !canGoUp) speed = 0; if (speed < 0 && !canGoDown) speed = 0; chainLift.SetSpeed(speed); if (tick >50) { if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) { //Smart Dash outputs //SmartDashboard::PutNumber("X Acceleration: ", accel.GetX()); //SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY()); //SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ()); SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get()); SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get()); SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get()); SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get()); SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get()); //SmartDashboard::PutBoolean("RobotDrive Alive?", // robotDrive.IsAlive()); //SmartDashboard::PutBoolean("ChainLift Alive?", // robotDrive.IsAlive()); SmartDashboard::PutNumber("Left Sensor", leftIR.GetAverageVoltage()); SmartDashboard::PutNumber("Right Sensor", rightIR.GetAverageVoltage()); SmartDashboard::PutNumber("Left w zero", leftIR.GetAverageVoltage() - leftIRZero); SmartDashboard::PutNumber("Rigt w zero", rightIR.GetAverageVoltage() - rightIRZero); SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14)); SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15)); } tick = 0; } }
void SwerveDrive::Update(Joystick &stick, Joystick &stick2, float gyroValue) { float zInput = stick.GetZ(); // Rotation Clockwise float xInput = stick.GetX(); // Strafe float yInput = stick.GetY(); // Forward float temp = yInput * cos(gyroValue * M_PI / 180) + xInput * sin(gyroValue * M_PI / 180); // This block of commands SHOULD make this thing field oriented xInput = -yInput * sin(gyroValue * M_PI / 180) + xInput * cos(gyroValue * M_PI / 180); yInput = temp; int *controlType; controlType = (int *)a_ControlTypeChooser.GetSelected(); if (controlType == NULL) { std::cout << "error reading control type" << std::endl; return; } float A = xInput - zInput * (a_RobotLength / a_ChassisRadius); float B = xInput + zInput * (a_RobotLength / a_ChassisRadius); float C = yInput - zInput * (a_RobotWidth / a_ChassisRadius); float D = yInput + zInput * (a_RobotWidth / a_ChassisRadius); float max = -9 * pow(10,4); double frSpeed = 0.0; double frAngle = 0.0; double flSpeed = 0.0; double flAngle = 0.0; double blSpeed = 0.0; double blAngle = 0.0; double brSpeed = 0.0; double brAngle = 0.0; switch (*controlType) { case CONTROL_TYPE_SWERVE: frSpeed = sqrt(pow(B,2) + pow(C,2)); flSpeed = sqrt(pow(B,2) + pow(D,2)); blSpeed = sqrt(pow(A,2) + pow(D,2)); brSpeed = sqrt(pow(A,2) + pow(C,2)); max = frSpeed; if(flSpeed > max) { max = flSpeed; } if(blSpeed > max) { max = blSpeed; } if(brSpeed > max) { max = brSpeed; } if(max > 1) { // This is done so that if a speed greater than 1 is calculated, all are reduced proportionally frSpeed /= max; flSpeed /= max; blSpeed /= max; brSpeed /= max; } // atan2 outputs values in a manner similar to what is shown on the below diagram //////////////////// // 0 // // // // // // // // // // //-90///////////90// // // // // // // // // // // -180 or 180 // //////////////////// frAngle = (atan2(C,B) * 180.0 / M_PI); flAngle = (atan2(D,B) * 180.0 / M_PI); blAngle = (atan2(D,A) * 180.0 / M_PI); brAngle = (atan2(C,A) * 180.0 / M_PI); // If on the left side, add 360 to normalize values to the below diagram //////////////////// // 360 or 0 // // // // // // // // // // //270///////////90// // // // // // // // // // // 180 // //////////////////// if(frAngle < 0) { frAngle += 360; } if(flAngle < 0) { flAngle += 360; } if(brAngle < 0) { brAngle += 360; } if(blAngle < 0) { blAngle += 360; } break; case CONTROL_TYPE_CRAB: float setAngle = atan2(yInput,xInput) * 180.0 / M_PI; // find the angle the stick is pointed to, use that for all wheels frAngle = setAngle; flAngle = setAngle; blAngle = setAngle; brAngle = setAngle; float setSpeed = sqrt(pow(xInput,2) + pow(yInput,2)); // find the r of the joystick vector, if you think about it in polar coordinates frSpeed = setSpeed; flSpeed = setSpeed; blSpeed = setSpeed; brSpeed = setSpeed; break; } a_FrontRight.Set(frAngle, frSpeed); a_FrontLeft.Set(flAngle, flSpeed); a_BackLeft.Set(blAngle, blSpeed); a_BackRight.Set(brAngle, brSpeed); }
void OperatorControl() { while (IsOperatorControl() && IsEnabled()) { robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY())); SmartDashboard::PutNumber("StickZ",stick.GetZ()); SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ())); finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y))); // scalerValue = stick.GetRawAxis(3); arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y))); manualShooter(); shootingModes(); toggleIntake(); toggleIntakeMode(); setScalerValue(); // double volts = ourRangefinder->GetVoltage(); // SmartDashboard::PutNumber("Voltage",volts); //t_motor.Set(stick2.GetZ()); //t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis)); // t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis)); // finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis)); // arm_Motor.Set(stick2.GetY()); // Current Control mode Debug SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent()); SmartDashboard::PutNumber("Position",t_motor.GetPosition()); // t_motor.Set(stick.GetAxis(Joystick::Slider)); // if (stick.GetRawButton(3) == true){ // t_motor.SetPosition(10000); // } // if (stick2.GetRawButton(7) == true and buttonpress == false){ // // } // else if (stick2.GetRawButton(3) == false and buttonpress == true){ // buttonpress = false;// drive with arcade style (use right stick) // // } SmartDashboard::PutBoolean("buttonpress state",buttonpress); if (stick2.GetRawButton(buttonBack) == true){ stateDisarmed = true; stateArming1 = false; stateArming2 = false; stateArmed = false; stateFiring1 = false; stateFiring2 = false; t_motor.SetPosition(0); } toggleIntake(); // if (stick2.GetRawButton(5) == true){ // intake_Spin_Motor.Set(1); // } // if (stick2.GetRawButton(5) == false){ // intake_Spin_Motor.Set(0); // } if (stick.GetRawButton(2) == true and buttonpress2 == false){ myServo->SetAngle(175); } if (stick.GetRawButton(2) == false and buttonpress2 == true){ myServo->SetAngle(5); } // servoSetPos(servoState); // myServo->Set(.5); Wait(0.005);// wait for a motor update time // motorSetPos(launcherState); } }
void TeleopPeriodic(void) { // increment the number of teleop periodic loops completed GetWatchdog().Feed(); m_telePeriodicLoops++; /* * No longer needed since periodic loops are now synchronized with incoming packets. if (m_ds->GetPacketNumber() != m_priorPacketNumber) { */ /* * Code placed in here will be called only when a new packet of information * has been received by the Driver Station. Any code which needs new information * from the DS should go in here */ m_dsPacketsReceivedInCurrentSecond++; // increment DS packets received // put Driver Station-dependent code here // Demonstrate the use of the Joystick buttons // DemonstrateJoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick", &m_solenoids[1]); // DemonstrateJoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", &m_solenoids[5]); /*** // determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick if (m_rightStick->GetZ() <= 0) { // Logitech Attack3 has z-polarity reversed; up is negative // use arcade drive m_robotDrive->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick) if (m_driveMode != ARCADE_DRIVE) { // if newly entered arcade drive, print out a message printf("Arcade Drive\n"); m_driveMode = ARCADE_DRIVE; } } else { // use tank drive m_robotDrive->TankDrive(m_leftStick, m_rightStick); // drive with tank style if (m_driveMode != TANK_DRIVE) { // if newly entered tank drive, print out a message printf("Tank Drive\n"); m_driveMode = TANK_DRIVE; } } ***/ /* Grab z-wheel value and transform from [1, -1] to [0,4600] * 4600 rpm is a guess */ float rawZ, transformedZ; rawZ = m_leftStick->GetZ(); // transformedZ = (1.0 - rawZ)/(-2.0); transformedZ = -2300.0 * rawZ + 2300.0; /* Driver station display output. */ char msg[256]; static DriverStationLCD *dsLCD = DriverStationLCD::GetInstance(); sprintf(msg, "Launcher Speed = %f RPM", transformedZ); dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, msg); sprintf(msg, "Loader Limit = %u", m_loaderLimit->Get()); dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, msg); sprintf(msg, "Loader Trigger = %u", m_leftStick->GetTrigger()); dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, msg); // line number (enum), starting col, format string, args for format string ... dsLCD->UpdateLCD(); // use arcade drive //m_driveGain = (1.0 - m_rightStick->GetZ())/(-2.0); m_robot->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick) // m_robotFrontDrive->Drive(-m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn // m_robotRearDrive->Drive(m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn // -Add an elevation control, add a motor controller to port 8, and add a joystick button pair to control up/down if (m_rightStick->GetRawButton(m_elevatorUpButton)){ m_elevatorMotor->Set(m_elevatorUpSpeed); } else if (m_rightStick->GetRawButton(m_elevatorDownButton)){ m_elevatorMotor->Set(m_elevatorDownSpeed); } else { m_elevatorMotor->Set(0.0); } // trigger button activates feeding and loading mechanisms if ((m_leftStick->GetTop() || m_feeding) && (!m_loading) && (m_feedCounter <= m_feedCount)){ m_feeding = true; m_feedCounter++; m_feedMotor->Set(m_feedSpeed); } else { m_feeding = false; m_feedCounter = 0; m_feedMotor->Set(0.); } m_loaderLimit->Get(); //just testing digital input port 1 // top(2) button uses the Z wheel to control the speed of the loading mechanisms if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (m_loadCounter <= m_loadCount)){//comment out this line // if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (bool) m_loaderLimit->Get()){ //uncomment this line m_loading = true; m_loadCounter++; // comment out this line m_loadMotor->Set(m_loadSpeed); // load breech } else if ((m_loading && (!m_feeding)) && (m_loadCounter > m_loadCount) && (m_loadCounter <= ((m_loadCount+m_loadPauseCount)))) { // comment out the pause logic m_loading = true;// comment out the pause logic m_loadCounter++;// comment out the pause logic, m_loadMotor->Set(0.); // pause loader // comment out the pause logic } // else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loadCounter<= 2*m_loadCount-13)) { // comment out this line else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loaderLimit->Get()!=0)) { //Retracting loader arm (feeder), have not yet hit the limit switch. // else if ((m_loading &! m_feeding) && (bool) m_loaderResetLimit->Get()) { // uncomment this line m_loading = true; m_loadCounter++; // comment out the pause logic m_loadMotor->Set(-m_loadSpeed);; // reset loader } else { //Finished retracting loader arm (feeder), so stop it. m_loading = false; m_loadCounter = 0; m_loadMotor->Set(0.);; // stop loader } // Set launcher motor command m_launchMotor->Set(-(1.0 - m_leftStick->GetZ())/(-2.0)); // transform from [-1.0, +1.0] to [0.0, +1.0] /* } // if (m_ds->GetPacketNumber()... */ // use buttons 8 and 9 to trim load motor position in maintenance mode /*if (m_leftStick->GetRawButton(8) && (!m_feeding) && (!m_loading)){ m_loadMotor->Set(0.12); } else if (m_leftStick->GetRawButton(9) && (!m_feeding) && (!m_loading)){ m_loadMotor->Set(-0.1); }*/ //Use buttons 10 and 11 to trim feed motor position in maintenance mode if (m_leftStick->GetRawButton(11) && (!m_feeding) && (!m_loading)){ m_feedMotor->Set(0.15); } else if (m_leftStick->GetRawButton(10) && (!m_feeding) && (!m_loading)){ m_feedMotor->Set(-0.1); } } // TeleopPeriodic(void)