Beispiel #1
0
void QuatIMU_UpdateControls( RADIO * Radio , int ManualMode )
{
  ((int*)IMU_VARS)[In_Elev] = Deadband( Radio->Elev, 24 );
  ((int*)IMU_VARS)[In_Aile] = Deadband( Radio->Aile, 24 );
  ((int*)IMU_VARS)[In_Rudd] = Deadband( Radio->Rudd, 24 );

  if( ManualMode ) {
    F32::RunStream( UpdateControls_Manual , IMU_VARS );
  }
  else {
    F32::RunStream( UpdateControlQuaternion_AutoLevel , IMU_VARS );
  }

  F32::WaitStream();
  F32::RunStream( UpdateControls_ComputeOrientationChange , IMU_VARS );
}
void DriveTrain::Drive(Joystick* joy) {
#if JOYTYPE == XBOX_GAMEPAD
	double left=-joy->GetRawAxis(1);
	double right=-joy->GetRawAxis(4);
#else
	double left=-joy->GetY();
	double right=-joy->GetRawAxis(4);
#endif
	if(reverse_driving){
		double r=right;
		double l=left;
		left=-r;
		right=-l;
	}
	//std::cout<<"l:"<<left<<" r:"<<right<<std::endl;
	Limit(left);
	Limit(right);
	if(squared_inputs)
		SquareInputs(left,right);
	left=Deadband(left,x_deadband);
	right=Deadband(right,y_deadband);
	Drive(left,right);
}
Beispiel #3
0
bool FGActuator::Run(void )
{
  Input = InputNodes[0]->getDoubleValue() * InputSigns[0];

  if( fcs->GetTrimStatus() ) initialized = 0;

  if (fail_zero) Input = 0;
  if (fail_hardover) Input =  clipmax*sign(Input);

  Output = Input; // Perfect actuator. At this point, if no failures are present
                  // and no subsequent lag, limiting, etc. is done, the output
                  // is simply the input. If any further processing is done
                  // (below) such as lag, rate limiting, hysteresis, etc., then
                  // the Input will be further processed and the eventual Output
                  // will be overwritten from this perfect value.

  if (fail_stuck) {
    Output = PreviousOutput;
  } else {
    if (lag != 0.0)              Lag();        // models actuator lag
    if (rate_limit_incr != 0 || rate_limit_decr != 0) RateLimit();  // limit the actuator rate
    if (deadband_width != 0.0)   Deadband();
    if (hysteresis_width != 0.0) Hysteresis();
    if (bias != 0.0)             Bias();       // models a finite bias
    if (delay != 0)              Delay();      // Model transport latency
  }

  PreviousOutput = Output; // previous value needed for "stuck" malfunction
  
  initialized = 1;

  Clip();

  if (clip) {
    saturated = false;
    if (Output >= clipmax && clipmax != 0) saturated = true;
    else if (Output <= clipmin && clipmin != 0) saturated = true;
  }

  if (IsOutput) SetOutput();

  return true;
}
Beispiel #4
0
/**
 * Get the value of the axis, applying invert and deadband
 * @return Value of the Axis
 */
float Axis::Get() {
	return (invert ? -1 : 1) * Deadband(stick->GetRawAxis(axisChannel), deadband);
}
Beispiel #5
0
		void OperatorControl (void) {
			GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
			
			DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
			
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
			
			SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
			SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
			SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
			
			while (IsOperatorControl() && IsEnabled()) {
				GetWatchdog().Feed(); // Feed the Watchdog.
				
				kinectMode = (bool) kinectModeSelector->GetSelected();
				demoMode = (bool) demoModeSelector->GetSelected();
				speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
				
				if (kinectMode) {
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
					
					if (!demoMode) {
						if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else {
							motorDriveLeft.Set(0);
							motorDriveRight.Set(0);
						}
					}
			
					if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER);
						motorFeed.Set(FEED_MOTOR_POWER);
						motorPickup.Set(PICKUP_MOTOR_POWER);
					} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
						motorFeed.Set(FEED_MOTOR_POWER * -1);
						motorPickup.Set(PICKUP_MOTOR_POWER * -1);
					} else {
						motorShooter.Set(0);
						motorFeed.Set(0);
						motorPickup.Set(0);
					}
				
					if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
						motorTurret.Set(TURRET_POWER);
					} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
						motorTurret.Set(TURRET_POWER * -1);
					} else {
						motorTurret.Set(0);
					}
				} else {
					if (joystickDriveLeft.GetThrottle() == 0) {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
						
						motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER);
							motorFeed.Set(FEED_MOTOR_POWER);
							motorPickup.Set(PICKUP_MOTOR_POWER);
						} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
							motorFeed.Set(FEED_MOTOR_POWER * -1);
							motorPickup.Set(PICKUP_MOTOR_POWER * -1);
						} else {
							motorShooter.Set(0);
							motorFeed.Set(0);
							motorPickup.Set(0);
						}
						
						if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
							motorTurret.Set(TURRET_POWER);
						} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
							motorTurret.Set(TURRET_POWER * -1);
						} else {
							motorTurret.Set(0);
						}
					} else {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
						
						motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
							motorShooter.Set(SHOOTER_MOTOR_POWER);
							motorFeed.Set(FEED_MOTOR_POWER);
							motorPickup.Set(PICKUP_MOTOR_POWER);
						} else if (joystickManipulator.GetRawButton(MANIPULATOR_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
							motorFeed.Set(FEED_MOTOR_POWER * -1);
							motorPickup.Set(PICKUP_MOTOR_POWER * -1);
						} else {
							motorShooter.Set(0);
							motorFeed.Set(0);
							motorPickup.Set(0);
						}
						
						motorTurret.Set(joystickManipulator.GetX() * -1 * TURRET_POWER);
					}
				}
				
				dsLCD->UpdateLCD();
			}

			GetWatchdog().SetEnabled(false); // Teleop is done, so let's turn off Watchdog.
		}