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