How to Programming

Made for FRC Team 972: Iron Claw, in Los Gatos High School, CA.
Authored by @whackamadoodle3000, @me1234q

Contents

Guides
  Motors
  Joysticks
  Encoders
  Built-in Accelerometer
  NavX
  Solenoids
  Double Solenoids
  Compressors
  Limit Switches

Examples
  Make Motor Spin Based on Joystick Input
  Arcade Drive
  Tank Drive
  PID Example Snippet
  Toggling Double Solenoids

Guides

Use Motors

Motors are one of the most common actuators and provide rotational motion. When given power, the motor will spin with speed proportional to the voltage and torque proportional to the current. They are controlled with a talon.

• To import
  • To import TalonSRX import com.ctre.phoenix.motorcontrol.can.*;
  • To import control modes import com.ctre.phoenix.motorcontrol.ControlMode;
• To initialize
  • To initialize motor WPI_TalonSRX Motor = new WPI_TalonSRX(6);
  • To initialize automatic deadband Motor.enableDeadbandElimination(true);
• To use (with various control modes)
  • Set motor absolute power Motor.set(ControlMode.PercentOutput, value); with value as double between -1 and 1
  • Set motor to follow another Motor.set(ControlMode.Follower, value); with value as the id of the other talon
• Documentation: http://www.ctr-electronics.com/downloads/api/java/html/classcom_1_1ctre_1_1phoenix_1_1motorcontrol_1_1can_1_1_w_p_i___talon_s_r_x.html

Use Joysticks

Joysticks are (misleadingly) an umbrella term for all user input devices, including gamepads, joysticks, etc. Joystick objects can receive joystick and button input.

• To import import edu.wpi.first.wpilibj.Joystick;
• To initialize Joystick Joy = new Joystick(0);
• To use
  • To get axis values Joy.getRawAxis(0); with value between -1 and 1
  • To get button values Joy.getRawButton(0); with boolean output
  • To get angle in radians Math.atan2(xAxis, -yAxis)
    • To get angle in degrees Math.atan2(xAxis, -yAxis) * 180 / Math.PI
  • To get magnitude Math.sqrt(xAxis*xAxis + yAxis*yAxis);
• Documentation: http://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/Joystick.html

Use External Encoders

Encoders are used for measuring rotation. This is done through counting the number of "clicks", which increments or decrements with every slight rotation.

• To import import edu.wpi.first.wpilibj.Encoder
• To initialize Encoder sampleEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
  • To set max period (secs) without clicks before it is considered at rest sampleEncoder.setMaxPeriod(.1);
  • Minimum rate (rotation/time) before device is stopped sampleEncoder.setMinRate(10);
  • How much distance is traveled every encoder pulse sampleEncoder.setDistancePerPulse(5);
  • Reverse the direction encoder counts sampleEncoder.setReverseDirection(true);
  • How many samples to take (1-127) when determining the periodsampleEncoder.setSamplesToAverage(7);
• To use
  • To get count of clicks int count = sampleEncoder.get();
  • To get distance double distance = sampleEncoder.getDistance();
  • To get period between clicks double period = sampleEncoder.getPeriod();
  • To get speed double rate = sampleEncoder.getRate();
  • To get current direction boolean direction = sampleEncoder.getDirection();
  • To see if the encoder is stopped turning boolean stopped = sampleEncoder.getStopped();
• Documentation: http://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/Encoder.html
• More info: https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599717-encoders-measuring-rotation-of-a-wheel-or-other-shaft

Use Talon Encoders

These encoders function the same as external encoders, but are connected to the talon's encoder port and thus can be directly accessed from the talon object.

• To import import com.ctre.phoenix.motorcontrol.FeedbackDevice;
• To initialize talon.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 100);
• To use
  • talon.getSensorCollection().getQuadraturePosition();
  • talon.getSensorCollection().getQuadratureVelocity();
• Documentation: http://www.ctr-electronics.com/downloads/api/java/html/classcom_1_1ctre_1_1phoenix_1_1motorcontrol_1_1_sensor_collection.html

Use the Built-in Accelerometer

This is the RoboRio's built-in accelerometer. It allows you to get the acceleration in the x, y, and z directions.

• To import import edu.wpi.first.wpilibj.BuiltInAccelerometer;
• To initialize BuiltInAccelerometer accel = new BuiltInAccelerometer();
• To use
  • Get x acceleration accel.getX()
  • Get y acceleration accel.getY()
  • Get z accelerationaccel.getZ()
• Documentation: http://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.html

Use the Kauai Labs NavX

The NavX gives a variety of information on the robot state, including orientation, velocity, acceleration, position, and more.

• To import
  • import com.kauailabs.navx.frc.AHRS;
  • import edu.wpi.first.wpilibj.SPI; or I2C
• To initialize
  • If mounted on RoboRIO AHRS ahrs = new AHRS(SPI.Port.kMXP);
  • If mounted elsewhere AHRS ahrs = new AHRS(I2C.Port.kMXP);
• To use
  • Orientation Data
    • To get heading ahrs.getAngle()
    • To get yaw ahrs.getYaw() (-180 to 180 degrees)
    • To get pitch ahrs.getPitch() (-180 to 180 degrees)
    • To get roll ahrs.getRoll()
    • To get compass data ahrs.getCompassHeading() (0 to 360)
  • Velocity Data in Meters/Sec
    • Velocity in x direction ahrs.getVelocityX()
    • Velocity in y direction ahrs.getVelocityY()
    • Velocity in z direction ahrs.getVelocityZ()
    • Rate of yaw (turning) ahrs.getRate()
  • Acceleration Data in g-force
    • Acceleration in x direction ahrs.getWorldLinearAccelX()
    • Acceleration in y direction ahrs.getWorldLinearAccelY()
    • Acceleration in z direction ahrs.getWorldLinearAccelZ()
  • Distance/Displacement Data in meters
    • Displacement x ahrs.getDisplacementX()
    • Displacement y ahrs.getDisplacementY()
    • Displacement z ahrs.getDisplacementZ()
  • Pressure and Temperature data
    • Barometric Pressure in millibars ahrs.getBarometricPressure()
    • Temperature in Celsius ahrs.getTempC()
  • Boolean Motion Data
    • To see if it is rotating ahrs.isRotating()
    • To see if it is moving ahrs.isMoving()
  • Reset
    • To reset measurements for the yaw gyro ahrs.reset()
    • To zero yaw gyro ahrs.zeroYaw() (tell it what forward mean)
    • To reset displatement distances ahrs.resetDisplacement()
  • Board Info
    • To get update rate ahrs.getActualUpdateRate()
    • Check if it is connected ahrs.isConnected()
• Documentation: https://www.kauailabs.com/public_files/navx-mxp/apidocs/java/com/kauailabs/navx/frc/AHRS.html

Use Single Acting Solenoids

Solenoids are the most often used actuator for linear motion. Single acting Solenoids extend by increasing the air pressure inside a piston. Double Solenoids are recommended, as their have two channels, allowing them to more easily switch between extended and retracted position.

• To import import edu.wpi.first.wpilibj.Solenoid;
• To initialize Solenoid solenoid = new Solenoid(1)
• To use
  • To turn on solenoid.set(true)
  • To turn off solenoid.set(false)
  • To get solenoid state in boolean solenoid.get()
• Documentation: http://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/Solenoid.html

Use a Double Solenoid

These are similar to single acting solenoids, but have a second channel allowing them to revert to their original position without the need of an external force.

• To import import edu.wpi.first.wpilibj.DoubleSolenoid;
• To initialize DoubleSolenoid doubleSolenoid = new DoubleSolenoid(forwardchannel, reversechannel);
• To use
  • To block all pressure doubleSolenoid.set(DoubleSolenoid.Value.kOff);
  • To put pressure in forward channel doubleSolenoid.set(DoubleSolenoid.Value.kForward);
  • To put pressure in reverse channel doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
• Documentation: http://first.wpi.edu/FRC/roborio/release/docs/java/edu/wpi/first/wpilibj/DoubleSolenoid.html

Use a Compressor

Compressors are used for compressing air. They are need for any pneumatics, as they need air at a high pressure to function.

• To import import edu.wpi.first.wpilibj.Compressor;
• To initialize Compressor c = new Compressor(40);
• To control
  • To toggle closed loop control (goes up until maximum PSI) c.setClosedLoopControl(boolean);
  • To turn on c.start();
  • To turn off c.stop();
• Getting information
  • To get if the pressure is low in boolean c.getPressureSwitchValue()
  • To get the current being consumed in amps in double c.getCompressorCurrent()
  • To check if closed loop control is on c.getClosedLoopControl()
• Getting fault/error information
  • Check if compressor is disabled because current is too high c.getCompressorCurrentTooHighFault()
  • Check if the compressor is disabled because output is shorted c.getCompressorShortedFault()
  • Check if compressor is is not connected/not drawing enough current c.getCompressorNotConnectedFault()
• Documentation: http://first.wpi.edu/FRC/roborio/beta/docs/java/edu/wpi/first/wpilibj/Compressor.html

Use a Limit Switch

Limit switches are devices that mechanically prevent an actuator from extending a certain predetermined position.

• To import import edu.wpi.first.wpilibj.DigitalInput;
• To initialize DigitalInput limitSwitch = new DigitalInput(1);
• To get value limitSwitch.get() (boolean)
• Documentation: http://first.wpi.edu/FRC/roborio/beta/docs/java/edu/wpi/first/wpilibj/DigitalInput.html

Examples

Make Motor Spin based on Joystick Input　

package org.usfirst.frc.team972.robot;

import com.ctre.phoenix.motorcontrol.can.*;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;

public class Robot extends IterativeRobot {
	WPI_TalonSRX motor = new WPI_TalonSRX(6); //initialize motor
	Joystick joy = new Joystick(0); //initialize joystick (to find number, check driver station)
	double powerMultiplier = 0.7; //scales down motor power

	public void robotInit() {
		//This function is run when the robot is first started up
	}

	public void teleopPeriodic() {
		//This function is called periodically during teleoperated control
		double joystickValue = joy.getRawAxis(1); //find axis number in driver station
		System.out.println("Joy: " + joystickValue);
		Motor.set(joystickValue*powerMultiplier); //scale down by powerMultiplier
	}
}

Arcade Drive　

package org.usfirst.frc.team972.robot;

import com.ctre.phoenix.motorcontrol.can.*;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;

public class Robot extends IterativeRobot {

	/* talons for arcade drive */
	WPI_TalonSRX _frontLeftMotor = new WPI_TalonSRX(6);
	WPI_TalonSRX _frontRightMotor = new WPI_TalonSRX(2);

	/* extra talons and victors for six motor drives */
	WPI_TalonSRX _leftSlave1 = new WPI_TalonSRX(5);
	WPI_VictorSPX _rightSlave1 = new WPI_VictorSPX(7);
	WPI_TalonSRX _leftSlave2 = new WPI_TalonSRX(4);
	WPI_VictorSPX _rightSlave2 = new WPI_VictorSPX(17);

	DifferentialDrive _drive = new DifferentialDrive(_frontLeftMotor, _frontRightMotor);

	Joystick _joy = new Joystick(0);

	/**
	 * This function is run when the robot is first started up and should be
	 * used for any initialization code.
	 */
	public void robotInit() {
		/*
		 * take our extra talons and just have them follow the Talons updated in
		 * arcadeDrive
		 */
		_leftSlave1.follow(_frontLeftMotor);
		_leftSlave2.follow(_frontLeftMotor);
		_rightSlave1.follow(_frontRightMotor);
		_rightSlave2.follow(_frontRightMotor);

		/* drive robot forward and make sure all 
		 * motors spin the correct way.
		 * Toggle booleans accordingly.... */
		_frontLeftMotor.setInverted(false);
		_leftSlave1.setInverted(false);
		_leftSlave2.setInverted(false);
		
		_frontRightMotor.setInverted(false);
		_rightSlave1.setInverted(false);
		_rightSlave2.setInverted(false);
	}

	/**
	 * This function is called periodically during operator control
	 */
	public void teleopPeriodic() {
		double forward = -1.0 * _joy.getY();
		/* sign this so right is positive. */
		double turn = +1.0 * _joy.getZ();
		/* deadband */
		if (Math.abs(forward) < 0.10) {
			/* within 10% joystick, make it zero */
			forward = 0;
		}
		if (Math.abs(turn) < 0.10) {
			/* within 10% joystick, make it zero */
			turn = 0;
		}
		/* print the joystick values to sign them, comment
		 * out this line after checking the joystick directions. */
		System.out.println("JoyY:" + forward + "  turn:" + turn );
		/* drive the robot, when driving forward one side will be red.  
		 * This is because DifferentialDrive assumes 
		 * one side must be negative */
		_drive.tankDrive(forward, turn);
	}
}

Tank Drive　

package org.usfirst.frc.team972.robot;

import com.ctre.phoenix.motorcontrol.can.*;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;

public class Robot extends IterativeRobot {

	/* talons for arcade drive */
	WPI_TalonSRX _frontLeftMotor = new WPI_TalonSRX(6);
	WPI_TalonSRX _frontRightMotor = new WPI_TalonSRX(2);

	/* extra talons and victors for six motor drives */
	WPI_TalonSRX _leftSlave1 = new WPI_TalonSRX(5);
	WPI_VictorSPX _rightSlave1 = new WPI_VictorSPX(7);
	WPI_TalonSRX _leftSlave2 = new WPI_TalonSRX(4);
	WPI_VictorSPX _rightSlave2 = new WPI_VictorSPX(17);

	DifferentialDrive _drive = new DifferentialDrive(_frontLeftMotor, _frontRightMotor);

	Joystick leftJoy = new Joystick(0);
	Joystick rightJoy = new Joystick(1);

	//This function is run when the robot is first started up and should be used for any initialization code.
	
	public void robotInit() {
		//take our extra talons and just have them follow the Talons updated in
		_leftSlave1.follow(_frontLeftMotor);
		_leftSlave2.follow(_frontLeftMotor);
		_rightSlave1.follow(_frontRightMotor);
		_rightSlave2.follow(_frontRightMotor);

		/* drive robot forward and make sure all 
		 * motors spin the correct way.
		 * Toggle booleans accordingly.... */
		_frontLeftMotor.setInverted(false);
		_leftSlave1.setInverted(false);
		_leftSlave2.setInverted(false);
		
		_frontRightMotor.setInverted(false);
		_rightSlave1.setInverted(false);
		_rightSlave2.setInverted(false);
	}

	/**
	 * This function is called periodically during operator control
	 */
	public void teleopPeriodic() {
	
		double right = rightJoy.getY();
		double left =  leftjoy.getY();
		/* deadband */
		if (Math.abs(left) < 0.10) {
			/* within 10% joystick, make it zero */
			left = 0;
		}
		if (Math.abs(right) < 0.10) {
			/* within 10% joystick, make it zero */
			right = 0;
		}
		/* print the joystick values to sign them, comment
		 * out this line after checking the joystick directions. */
		System.out.println("Left: " + left + "  Right:" + right );
		_drive.tankDrive(left, right);
	}
}

PID Example Snippet

//At top:


double proportionFactor = 0.2; //These values should be tuned to be the most efficient.
double integralFactor = 0.2;
double derivativeFactor = 0.2;
double marginOfError = 0.01;

double actual = 1; //Change based on situation
double desired = 10;

double priorActual = actual;
double integral = 0;
double error = desired - actual;

Repeated loop (like slow or fast periodic) {
	priorActual = actual;
	actual = <get sensor data>;
	error = desired - actual;
	if (Math.abs(desired-actual) < marginOfError){
		<End of PID, Position Achieved>
	} else {
		integral = integral + current - desired;
	}
	return proportionFactor*error + integralFactor*integral + derivativeFactor*(actual-priorActual);
}

Toggling Double Solenoids

package org.usfirst.frc.team972.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.*;

public class Robot extends IterativeRobot {

	Compressor com = new Compressor(40);
	DoubleSolenoid sol = new Double Solenoid(1, 2);

	Joystick joy = new Joystick(1);

	//This function is run when the robot is first started up and should be used for any initialization code.
	
	public void robotInit() {
		//set up compressors
		com.setClosedLoopControl(true);
		com.start();
	}

	
	public void teleopPeriodic() {
		// Called when the button was released since last check
		if(joystick.getRawButtonReleased(0)) {
			if(frontSolenoid.get().equals(kForward)) {
				frontSolenoid.set(kReverse);
			} else if(frontSolenoid.get().equals(kReverse)) {
				frontSolenoid.set(kForward);
			}
			
		}
	}
}

---

Last Edit on January 26, 2019