C++ Joystick::GetZ方法代码示例

	/**
	 * 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)
    }

   /**
    * 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        
                       }
           
//.........这里部分代码省略.........