コード例 #1
0
ファイル: Robot.cpp プロジェクト: Kartazio/navxmxp
    /**
     * 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
        }
    }
コード例 #2
0
ファイル: Robot.cpp プロジェクト: Kartazio/navxmxp
 /**
  * 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
     }
 }
コード例 #3
0
ファイル: Robot.cpp プロジェクト: Kartazio/navxmxp
    /**
     * 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
        }
    }