Esempio n. 1
0
	/**
	 * Drive left & right motors for 2 seconds, enabled by a jumper (jumper
	 * must be in for autonomous to operate).
	 */
	void Autonomous(void) {
		myRobot->SetSafetyEnabled(false);
		if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1) // only run the autonomous program if jumper is in place
		{
			myRobot->Drive(0.5, 0.0); // drive forwards half speed
			Wait(2.0); //    for 2 seconds
			myRobot->Drive(0.0, 0.0); // stop robot\
		}
			myRobot->SetSafetyEnabled(true);
		}
Esempio n. 2
0
	void OperatorControl(void)
	{
		
		bool buttonABool;
		bool buttonBBool;
		bool buttonXBool;
		bool buttonYBool; 
		bool buttonLBBool;
		bool buttonRBBool;
		bool buttonBackBool;
		bool buttonStartBool;
		
		float leftTrigger;
		float rightTrigger;
		float leftYAxis;
		float rightYAxis;
		float leftXAxis;
		float rightXAxis;
		
		myRobot.SetSafetyEnabled(true);
		
		while (IsOperatorControl())
		{
			leftYAxis = newStick.GetLeftYAxis();
			rightYAxis = newStick.GetRightYAxis();
			SmartDashboard::PutNumber("Left Y Axis", leftYAxis);
			SmartDashboard::PutNumber("Right Y Axis", rightYAxis);
			
			leftXAxis = newStick.GetLeftXAxis();
			rightXAxis = newStick.GetRightXAxis();
			SmartDashboard::PutNumber("Left X Axis", leftXAxis);
			SmartDashboard::PutNumber("Right X Axis", rightXAxis);
						
			leftTrigger = newStick.GetLeftThrottle();
			SmartDashboard::PutNumber("left trigger", leftTrigger);

			rightTrigger = newStick.GetRightThrottle();
			SmartDashboard::PutNumber("right trigger", rightTrigger);
			
			buttonABool = newStick.GetButtonA();
			SmartDashboard::PutNumber("buttonA",buttonABool);
			buttonBBool = newStick.GetButtonB();
			SmartDashboard::PutNumber("buttonB",buttonBBool);
			buttonXBool = newStick.GetButtonX();
			SmartDashboard::PutNumber("buttonX",buttonXBool);
			buttonYBool = newStick.GetButtonY();
			SmartDashboard::PutNumber("buttonY",buttonYBool);
			
			buttonLBBool = newStick.GetButtonLB();
			SmartDashboard::PutNumber("buttonLB",buttonLBBool);
			buttonRBBool = newStick.GetButtonRB();
			SmartDashboard::PutNumber("buttonRB",buttonRBBool);
			buttonBackBool = newStick.GetButtonBack();
			SmartDashboard::PutNumber("buttonBack",buttonBackBool);
			buttonStartBool = newStick.GetButtonStart();
			SmartDashboard::PutNumber("buttonStart",buttonStartBool);
			
			Wait(0.005);				
		}
	}
Esempio n. 3
0
	void OperatorControl(void)
	{
		NetTest();
		return;
		myRobot.SetSafetyEnabled(true);
		digEncoder.Start();
		const double ppsTOrpm = 60.0/250.0;   //Convert from Pos per Second to Rotations per Minute by multiplication
                                              // (See the second number on the back of the encoder to replace 250 for different encoders)
        const float VoltsToIn = 41.0;         // Convert from volts to cm by multiplication (volts from ultrasonic).
                                              // This value worked for distances between 1' and 10'.
		
		while (IsOperatorControl())
		{
			if (stick.GetRawButton(4)) {
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
			} 
			else if (stick.GetRawButton(5))
			{
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
			}
			else 
			{
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
			}
			
			myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
			
			SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
			SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
			SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());

			Wait(0.1);
		}
		digEncoder.Stop();
	}
Esempio n. 4
0
	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
	}
Esempio n. 5
0
    /**
     * 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
        }
    }
Esempio n. 6
0
    // Real Autonomous
    //	* Code to be run autonomously for the first ten (10) seconds of the match.
    //	* Launch catapult
    //	* Drive robot forward ENCODER_DIST ticks.
    void Autonomous()
    {
        robotDrive.SetSafetyEnabled(false);

        // STEP 1: Set all of the states.
        // SAFETY AND SANITY - SET ALL TO ZERO
        loaded = winchSwitch.Get();
        loading = false;
        intake.Set(0.0);
        winch.Set(0.0);

        // STEP 2: Move forward to optimum shooting position
        Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);

        // STEP 3: Drop the arm for a clean shot
        arm.Set(DoubleSolenoid::kForward);
        Wait(1.0); // Ken

        // STEP 4: Launch the catapult
        LaunchCatapult();
        Wait (1.0); // Ken

        // Get us fully into the zone for 5 points
        Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);

        // SAFETY AND SANITY - SET ALL TO ZERO
        intake.Set(0.0);
        winch.Set(0.0);
    }
	//Choose which auto to use
	void AutonomousInit() {

		chainLift.SetSpeed(0);
		canGrabber.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "STARTING AUTO");
		robotDrive.SetSafetyEnabled(false);
		chainLift.SetSafetyEnabled(false);

		SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
		SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());

		//Select auto type
		if (autoSwitch1.Get()) {
			if (autoSwitch2.Get())
				AutonomousType4();
			else
				//1 on 2 grab n back
				AutonomousType8();
		} else {
			if (autoSwitch2.Get())
				//1 off, 2 on: grab n turn
				AutonomousType10();
			else {
				SmartAutoPicker();
			}
			//Do Nothing
		}
	}
Esempio n. 8
0
	/*this fuction will need to be updated for the final robot*/
	void FunctionBot::configDrive() {
		drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
		drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
		//drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor,true);
		drive->SetSafetyEnabled(false);
		//not sure on this value at all
		drive->SetMaxOutput(333);
	}
Esempio n. 9
0
	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			Wait(0.005);				// wait for a motor update time
		}
	}
Esempio n. 10
0
	Wheels()
	{
		drive = new RobotDrive(0,2,4,3); //frontLeft, rearLeft, frontRight, rearRight
		drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true); // 0 is front left wheel
		drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true); // 2 is back left wheel
		drive->SetExpiration(0.1);
		drive->SetSafetyEnabled(true);
		driveSafety = new MotorSafetyHelper(drive);
	}
Esempio n. 11
0
	void TeleopInit() override {
		drive->SetExpiration(200000);
		drive->SetSafetyEnabled(false);
		liftdown->Set(false);

		intake_hold = false;
		lastLiftPos = 0;
		manual = true;
	}
Esempio n. 12
0
    /**
     * 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
        }
    }
Esempio n. 13
0
	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl()
	{
		cout << "Hello operator!" << endl;
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			Wait(0.005);				// wait for a motor update time
		}
		cout << "Bye operator!" << endl;
	}
Esempio n. 14
0
	/**
	 * Code to be run during the remaining 2:20 of the match (after Autonomous())
	 */
	void OperatorControl()
	{
		/* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was 
		 * the only way we could get out robot code to work (reliably). Should this be set to false?
		 */ 
		robotDrive.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			robotDrive.ArcadeDrive(rightStick);
			Wait(0.005);
		}
	}
Esempio n. 15
0
    // Test Autonomous
    void TestAutonomous()
    {
        robotDrive.SetSafetyEnabled(false);

        // STEP 1: Set all of the states.
        // SAFETY AND SANITY - SET ALL TO ZERO
        loaded = winchSwitch.Get();
        loading = false;
        intake.Set(0.0);
        winch.Set(0.0);

        // STEP 2: Move forward to optimum shooting position
        Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);

        // STEP 3: Drop the arm for a clean shot
        arm.Set(DoubleSolenoid::kForward);
        Wait(1.0); // Ken

        // STEP 4: Launch the catapult
        LaunchCatapult();

        Wait (1.0); // Ken

        if (ds->GetDigitalIn(0))
        {
            // STEP 5: Start the intake motor and backup to our origin position to pick up another ball
            InitiateLoad();
            intake.Set(-INTAKE_COLLECT);
            while (CheckLoad());
            Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
            Wait(1.0);

            // STEP 6: Shut off the intake, bring up the arm and move to shooting position
            intake.Set(0.0);
            arm.Set(DoubleSolenoid::kReverse);
            Wait (1.0);
            Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);

            // Step 7: drop the arm for a clean shot and shoot
            arm.Set(DoubleSolenoid::kForward);

            // UNTESTED KICKED OFF FIELD
            Wait(1.0); // Ken
            LaunchCatapult();
        }

        // Get us fully into the zone for 5 points
        Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);

        // SAFETY AND SANITY - SET ALL TO ZERO
        intake.Set(0.0);
        winch.Set(0.0);
    }
Esempio n. 16
0
	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
			dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
			dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
			dsLCD->UpdateLCD();
			Wait(0.005);				// wait for a motor update time
		}
	}
Esempio n. 17
0
    // Code to be run during the remaining 2:20 of the match (after Autonomous())
    //
    // OperatorControl
    //	* Calls all the above methods
    void OperatorControl()
    {
        // SAFETY AND SANITY - SET ALL TO ZERO
        intake.Set(0.0);
        winch.Set(0.0);

        arm.Set(DoubleSolenoid::kReverse);

        /* TODO: Investigate. At least year's (GTR East) competition, we reached the conclusion that disabling this was
         * the only way we could get out robot code to work (reliably). Should this be set to false?
         */
        robotDrive.SetSafetyEnabled(false);

        Timer clock;
        int sanity = 0;
        int bigSanity = 0;

        loading = false;
        loaded = winchSwitch.Get();

        RegisterButtons();
        gamepad.Update();
        leftStick.Update();

        compressor.Start();

        while (IsOperatorControl() && IsEnabled())
        {
            clock.Start();

            HandleDriverInputs();
            HandleShooter();
            HandleArm();
            //			HandleEject();

            while (!clock.HasPeriodPassed(LOOP_PERIOD)); // add an IsEnabled???
            clock.Reset();
            sanity++;
            if (sanity >= 100)
            {
                bigSanity++;
                sanity = 0;
                dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "%d", bigSanity);
            }
            gamepad.Update();
            leftStick.Update();
            dsLCD->UpdateLCD();
        }

        // SAFETY AND SANITY - SET ALL TO ZERO
        intake.Set(0.0);
        winch.Set(0.0);
    }
Esempio n. 18
0
	/**
	 * 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
		}
	}
Esempio n. 19
0
	void OperatorControl()
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl() && IsEnabled())
		{
			//MECANUM DRIVE
			float yValue = stick->GetY();
			float xValue = stick->GetX();
			float rotation = stick->GetTwist();
			myRobot.MecanumDrive_Cartesian(xValue, yValue, rotation, 0.0);
			Wait(0.005);
		}
	}
Esempio n. 20
0
	/**
	 * Do auto stuff
	 */
	void Autonomous()
	{
		cout << "Hello autonomous!" << endl;
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(-0.125, 0.0); 	// drive forwards half speed
		Wait(5.0); 				//    for 5 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
		Wait(1.0);				//		Wait 1 sec
		myRobot.Drive(-0.125, 0.25);	//	Drive fwd @ 1/2 speed, 0.25 curve (right?)
		Wait(1.0);
		myRobot.Drive(0, 0);
		cout << "Bye autonomous!" << endl;
	}
Esempio n. 21
0
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		
		gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
		gamepad.EnableButton(BUTTON_COLLECTOR_REV);
		gamepad.EnableButton(BUTTON_SHOOTER);
		gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
		gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
		gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
		gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
		gamepad.EnableButton(BUTTON_STOP_ALL);
		gamepad.EnableButton(BUTTON_JOG_FWD);
		gamepad.EnableButton(BUTTON_JOG_REV);

		stick2.EnableButton(BUTTON_SHIFT);

		// Set inital states for all switches and buttons
		gamepad.Update();
		indexSwitch.Update();
		greenClawLockSwitch.Update();
		yellowClawLockSwitch.Update();
		
		stick2.Update();
		
		// Set initial states for all pneumatic actuators
		shifter.Set(DoubleSolenoid::kReverse);
		greenClaw.Set(DoubleSolenoid::kReverse);
		yellowClaw.Set(DoubleSolenoid::kReverse);

		compressor.Start ();
		
		while (IsOperatorControl())
		{
			gamepad.Update();
			stick2.Update();
			indexSwitch.Update();
			greenClawLockSwitch.Update();
			yellowClawLockSwitch.Update();
			
			HandleCollectorInputs();
			HandleDriverInputsManual();
			HandleArmInputs();
			HandleShooterInputs();
			HandleResetButton();
			UpdateStatusDisplays();
			
			dsLCD->UpdateLCD();
			Wait(0.005);				// wait for a motor update time
		}
	}
	void Test(void)
	{
		compressor->Start();
		myRobot.SetSafetyEnabled(true);
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(1);
		GetWatchdog().Feed();

		while(IsTest())
		{
			GetWatchdog().Feed();
			Wait(0.1);
		}
	}
	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void Autonomous()
	{
		std::string autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
			std::cout << "Running custom Autonomous" << std::endl;
			myRobot.SetSafetyEnabled(false);
			myRobot.Drive(-0.5, 1.0); 	// spin at half speed
			Wait(2.0); 				//    for 2 seconds
			myRobot.Drive(0.0, 0.0); 	// stop robot
		} else {
			//Default Auto goes here
			std::cout << "Running default Autonomous" << std::endl;
			myRobot.SetSafetyEnabled(false);
			myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
			Wait(2.0); 				//    for 2 seconds
			myRobot.Drive(0.0, 0.0); 	// stop robot
		}

	}
Esempio n. 24
0
	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl()
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl() && IsEnabled())
		{
			leftIntake.Set(0.8);
			rightIntake.Set(0.8);

			leftLift.Set(operatorStick.GetRawAxis(1));
			rightLift.Set(operatorStick.GetRawAxis(1));

			myRobot.TankDrive(driverStick.GetRawAxis(5), driverStick.GetRawAxis(1), true); // drive with arcade style (use right stick)
			Wait(0.005);				// wait for a motor update time
		}
	}
Esempio n. 25
0
	void OperatorControl(void)
	{
		myRobot->SetSafetyEnabled(false);
		
		LEDLights (true); //turn camera lights on
		
		shooterspeedTask->Start((UINT32)this); //start counting shooter speed
		
		kickerTask->Start((UINT32)this); //turns on the kicker task
		
		kicker_in_motion = false;
				
		while (IsOperatorControl() && !IsDisabled())
		{
			
			if (ControllBox->GetDigital(3)) //turn tracking on with switch 3 on controll box
			{ 
				tracking(ControllBox->GetDigital(7));
			}
			else 
			{
				myRobot->TankDrive(leftstick, rightstick); //if tracking is off, enable human drivers
				Wait(0.005);	// wait for a motor update time
			}

			Shooter_onoff=ControllBox->GetDigital(4); //shoot if switch 4 is on
		
			ballgatherer(ControllBox->GetDigital(5), rightstick->GetRawButton(10));
			 
			kicker_onoff=lonelystick->GetRawButton(1);
			
			bridgeboot(ControllBox->GetDigital(6));
			
			kicker_cancel=lonelystick->GetRawButton(2);
			
			//kicker_down=rightstick->GetRawButton(11));
			
		}
		
		
		LEDLights (false);
		shooterspeedTask->Stop();
		kickerTask->Stop();
		ballgatherer(false, false);
		kickermotor->Set(Relay::kOff);
	}
Esempio n. 26
0
 /**
  * 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
     }
 }
Esempio n. 27
0
	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl()
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl() && IsEnabled())
		{
			myRobot.ArcadeDrive(joystick); // drive with arcade style (use right stick)

			if(joystick.GetRawButton(1)) {
				crochets.Set(-0.6);
			}
			else if(joystick.GetRawButton(2)) {
				crochets.Set(0.6);
			}

			Wait(0.005);				// wait for a motor update time
		}
	}
	Robot() :
		robotDrive(Motor1, Motor2),	// these must be initialized in the same order
		stick(5),		// as they are declared above.
		lw(LiveWindow::GetInstance()),
		autoLoopCounter(0),
		Motor1(21),
		Motor2(12),
		Slave1(20),
		Slave2(14),
		t_motor(13),
		arm_Motor(23),
		finger_Motor(22),
		intake_Spin_Motor(11),
		intake_Winch_Motor(13),
		stick2(4),
		autoLoopCounter2(0)
	{
		robotDrive.SetExpiration(0.1);
		robotDrive.SetSafetyEnabled(false);
		Slave1.SetControlMode(CANSpeedController::kFollower);
		Slave1.Set(21);
		Slave2.SetControlMode(CANSpeedController::kFollower);
		Slave2.Set(12);
		Motor2.SetInverted(true); //12
		Slave2.SetInverted(true);//14
		arm_Motor.SetInverted(false);//23
		t_motor.SetInverted(true);//23
//		t_motor.SetControlMode(CANSpeedController::kVoltage);
//		t_motor.Set(0);
//		CameraServer::GetInstance()->SetQuality(50);

//		CameraServer::GetInstance()->SetSize(2);
//		//the camera name (ex "cam0") can be found through the roborio web interface
//		CameraServer::GetInstance()->StartAutomaticCapture("cam0");

		t_motor.SetControlMode(CANSpeedController::kPercentVbus);
//		t_motor.SetVoltageCompensationRampRate(24.0);
		t_motor.SetFeedbackDevice(CANTalon::QuadEncoder);
		t_motor.SetPosition(0);
//		t_motor.SetPID(1, 0, 0);
		arm_Motor.SetControlMode(CANSpeedController::kPercentVbus);
		finger_Motor.SetControlMode(CANSpeedController::kPercentVbus);
//		ourRangefinder = new AnalogInput(0);

	}
Esempio n. 29
0
	void Autonomous()
	{
		compressor.Start();//start compressor
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
		
		
		while (!shoot.Get()){//while shooter isnt cocked pull it back
			shooter.SetSpeed(1); //set motor
		}
		
		shooterSole.Set(shooterSole.kForward);//Sets Solenoid forward, shoot ball
		shooterSole.Set(shooterSole.kReverse);//Sets Solenoid backward
		
		myRobot.Drive(1.0, -1.0);//turn around for 0.5 seconds, we have to check to see if it takes that long
		Wait(0.5);//wait for 0.5 seconds
		myRobot.Drive(0.0, 0.0);//stop robot
	}
Esempio n. 30
0
	void Test() {
		robotDrive.SetSafetyEnabled(false);
		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] = 7; //send 0 to arduino
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		bool isSettingUp = true;

		pickup.setGrabber(-1);
		pickup.setLifter(1);
		while (isSettingUp) {
			isSettingUp = false;
			if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0);
			}
			else {
				isSettingUp = true;
			}

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0);
			}
			else {
				isSettingUp = true;
			}
		}
		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		toSend[0] = 8;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		while(IsTest() && IsEnabled());

		toSend[0] = 0;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
	}