###### tags: `程式組教程`

# Gyro

# Equipments

## Software Language

Program Language => JAVA

## Model and Brand

Gyro => navX-MXP

Introduce of gyro => [Link（New Page）](https://pdocs.kauailabs.com/navx-mxp/)

## DriveMode

MecanumDerive and TankDrive

## Value Setting

Normalmode Speed => （Both Side） x 0.5

Correction Angle Speed => （One Side） x 0.6 ， （Another Side） x 0.0

## Variable Name

SpeedControllerGroup => LeftMotor, RightMotor

Joystick => Joystick

Gyro => Gyro

Angle => CurrentAngle

Wheels =>

               Head => LimeLight Side
 LeftFront <= ||________|| => RightFront
               |  |  |  |
               |  |  |  |
               |__|__|__|
  LeftRear <= ||        || => RightRear

# Official Function

## GetAngle

We can use this to get the **cumulative** angle of gyro.

Data Type => Double

It will like this

YourGyroName.getAngle();

:::danger
**NOTE：**

Because this is **cumulative** , so we add some function to make sure its numerical value will between 0° to 360°
:::

## Reset

We use this function to reset our gyro.

Its code will like this

YourGyroName.reset();

## Reset

# Loop and Using

## Bottons

### Btn_Y

If you press botton Y , the angle of gyro will be set to zero immediately.
It's worth noting that , sometimes the botton might be insensitive , the solution of it is just press it more times.

the code of botton Y => [Link（Current Page）](##Botton_Y)

### Btn_X

We use botton X to correct robot angle to the angle we want（0°）.
When you want to use this function, you can **hold down** press botton X , and robot will turn automatically to the right direction

## Set Target Angle

We have gyro be set to zero when robot is just enabled.
If you want to reset the angle again aftre a drive you can press botton Y , which is on the joystick.

# Problems and Solutions

## Declare

When we try to declare the gyro we encountered some problems below.

### Install Library

When we are searching for gyro's software , we has been unable to learn the reason why declare code is unavailable.Therefore, we suspect that is it because the library hasn't been installed.
After we having this guess , we started to understand some library related information of the gyro.
At the beginning, we try a lot of offline library , but all of it is failed. Then, we find that there have another method which is using **"Online Library"**.The link below is the gyro's（NavX） library , just copy it to VS code "Manage Vendor Libraries".

Online Library Link => https://www.kauailabs.com/dist/frc/2021/navx_frc.json

### Import SPI

We encountered a problem which is the red line under "SPI" , which is declare code , can't be eliminated. It will like this.


The solution is import the word below to your code. And it should be eliminated.

import edu.wpi.first.wpilibj.SPI;

## Programming

### Robot Correction

After many attempts , we finally find the correct numerical value of robot correction loop. Also , we learned that the robot's variable is different from what we think.

Correct loop code => [Link（Current Page）](#Robot_Correction)

### Current Angle

Because the function which is given by *FRC* official is like this：

Gyro.getAngle()

The descripition of *FRC* official：

> Return the actual angle in degrees that the robot is currently facing.

>The angle is based on the current accumulator value corrected by the oversampling rate, the gyro type and the A/D calibration values. The angle is continuous, that is it will continue from 360 to 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from 360 to 0 on the second time around.

>The angle is expected to increase as the gyro turns clockwise when looked at from the top. It needs to follow NED axis conventions in order to work properly with dependent control loops.

>This heading is based on integration of the returned rate from the gyro.

By this , we know that the angle of the function is continuous. Therefore , we use some easy loop and a variable called "CurrentAngle" to deal with this problem.

Correct loop code => [Link（Current Page）](#Current_angle)

## Movement

### Drift

Despite we want to reduce the deviate to as small as possible , it still have some small amount of error.
We guess it is because of the friction on the ground.

### Angle

Although we hope the robot error can be as low as possible , it still have some error.
The reason why we don't directly set the range in the corrected loop to the minimum is because it is very difficlt to ask robot to stop exactly on the angle which we want.
What we can do is to minimize the range of loop. In the current test we know the smallest range is 0° ± 5 degrees.

# Code

## Setting and declaration

## Botton_Y

We write it in the part of **teleopPeriodic** to control continuously it by Joystick.

@Override
public void teleopPeriodic() {
    
   // Gyro Reset
  if( Joystick.getRawButton(Btn_Y) ){
    Gyro.reset();
  }
  
}

## Robot_Correction

We write it in the part of **robotPeriodic** because we want it can continuous work.

The idea of this loop is： If robot is stray from the right direction, which is 0° , we can use the botton X on the joystick to have it turn automatically to the right direction.
Furthermore , we use positive and negative number to correct the robot.

The reason why we write " CurrentAngle >= 0 + 5 " and "CurrentAngle >= 0 + 5" is because it will be easier for me to identify.

@Override
public void robotPeriodic() {

  //Turn Robot Angle Autonomously By Gyro
  if( Joystick.getRawButton( Btn_X ) ){
    if( CurrentAngle >= 0 + 5 ){  // easier to identify
      LeftFront.set(-0.6);
      LeftRear.set(-0.6);
      RightFront.set(0.0);
      RightRear.set(0.0);
    }

    if( CurrentAngle <= 0 - 5 ){ // easier to identify
      LeftFront.set(0.6);
      LeftRear.set(0.6);
      RightFront.set(0.0);
      RightRear.set(0.0);
    }
  }
  
}

## Current_angle

We write it in the part of **robotPeriodic** because we want it can continuous work.
We write it in the part of **teleopInit** because we want to prove gyro will be set to zero when the robot just opened.

 @Override
  public void teleopInit() {
    Gyro.reset();
  }

@Override
public void robotPeriodic() {

  //Turn Robot Angle to Correct
  if( CurrentAngle >= 360 )  CurrentAngle -= 360;
  if( CurrentAngle <= -360 )  CurrentAngle += 360;
  
}

## Final Code

package frc.robot;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.kauailabs.navx.frc.AHRS;

import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.CvSource;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.networktables.NetworkTable;

public class Robot extends TimedRobot {

  // PWM channels
  public final int kLeftFrontChannel = 3;
  public final int kLeftRearChannel = 4;
  public final int kRightFrontChannel = 1;
  public final int kRightRearChannel = 2;
  public final int joystickPort = 0;
  public final int rightUltrasonicPort = 1;
  public final int rearUltrasonicPort = 0;

  // driver station ports
  public final int leftStick_X = 0;
  public final int leftStick_Y = 1;
  public final int rightStick_X = 4;
  public final int rightStick_Y = 5;
  public final int trigger_L = 2;
  public final int trigger_R = 3;
  public final int Btn_A = 1;
  public final int Btn_B = 2;
  public final int Btn_X = 3;
  public final int Btn_Y = 4;
  public final int Btn_LB = 5;
  public final int Btn_RB = 6;
  public final int Btn_LS = 9;  
  public final int Btn_RS = 10;

  // Motors
  WPI_TalonSRX LeftFront = new WPI_TalonSRX(kLeftFrontChannel);
  WPI_TalonSRX LeftRear = new WPI_TalonSRX(kLeftRearChannel);
  WPI_TalonSRX RightFront = new WPI_TalonSRX(kRightFrontChannel);
  WPI_TalonSRX RightRear = new WPI_TalonSRX(kRightRearChannel);

  // SpeedControllerGroup and DifferentialDrive
  SpeedControllerGroup LeftMotor = new SpeedControllerGroup(LeftFront, LeftRear);
  SpeedControllerGroup RightMotor = new SpeedControllerGroup(RightFront, RightRear);
  DifferentialDrive drive = new DifferentialDrive(LeftMotor, RightMotor);

  // Mecanumderive
  public MecanumDrive m_Drive;

  // Joystick
  Joystick Joystick = new Joystick(joystickPort);

  // Gyro 
  AHRS Gyro = new AHRS(SPI.Port.kMXP);

  // Variables (drivetrain)
  double xSpeed;
  double ySpeed;
  double zRotation;

  // Variables (gyro)
  double CurrentAngle;

  @Override
  public void robotInit() {
    
    // Drivetrain
    m_Drive = new MecanumDrive(LeftFront, LeftRear, RightFront, RightRear);

    // motors inverted
    RightFront.setInverted(true);
    RightRear.setInverted(true);
    LeftFront.setInverted(true);
    LeftRear.setInverted(true); 

  }

  @Override
  public void robotPeriodic() {

    //Turn Robot Angle to Correct
    CurrentAngle = Gyro.getAngle();
    if( CurrentAngle >= 360 )  CurrentAngle -= 360;
    if( CurrentAngle <= -360 )  CurrentAngle += 360;

    //Turn Robot Angle Autonomously By Gyro
    if( Joystick.getRawButton( Btn_X ) ){
      if( CurrentAngle >= 0 + 5 ){
        LeftFront.set(-0.6);
        LeftRear.set(0.0);
        RightFront.set(-0.6);
        RightRear.set(0.0);
      }

      if( CurrentAngle <= 0 - 5
       ){
        LeftFront.set(0.6);
        LeftRear.set(0.0);
        RightFront.set(0.6);
        RightRear.set(0.0);
      }
    }
      
    // Post To Smart Dashboard
    SmartDashboard.putNumber("CurrentAngle", CurrentAngle);
    SmartDashboard.putData("LeftFront", LeftFront);
    SmartDashboard.putData("LeftRear", LeftRear);
    SmartDashboard.putData("RightFront", RightFront);
    SmartDashboard.putData("RightRear", RightRear);
    SmartDashboard.putNumber("xSpeed", xSpeed);
    SmartDashboard.putNumber("ySpeed", ySpeed);
    SmartDashboard.putNumber("zRotation", zRotation);
  }

  @Override
  public void autonomousInit() {}

  @Override
  public void autonomousPeriodic() {}

  @Override
  public void teleopInit() {
    Gyro.reset();
  }

  @Override
  public void teleopPeriodic() {

    //Turn Robot Angle to Correct
    CurrentAngle = Gyro.getAngle();
    if (CurrentAngle >= 360)
      CurrentAngle -= 360;
    if(CurrentAngle <= -360)
      CurrentAngle += 360;

    // Gyro Reset
    if( Joystick.getRawButton(Btn_Y) ){
      Gyro.reset();
    }
    
    // Drivetrain
    ySpeed = Joystick.getRawAxis(leftStick_X) * 0.5;
    xSpeed = Joystick.getRawAxis(leftStick_Y) * 0.5;
    zRotation = -Joystick.getRawAxis(rightStick_X) * 0.5;

    // Mecanumderive
    m_Drive.driveCartesian(ySpeed, xSpeed, zRotation);

  }

  @Override
  public void disabledInit() {}

  @Override
  public void disabledPeriodic() {}

  @Override
  public void testInit() {}

  @Override
  public void testPeriodic() {}
}