View Single Post
  #6   Spotlight this post!  
Unread 23-02-2016, 14:49
joeojazz joeojazz is offline
Registered User
FRC #5712
 
Join Date: Jan 2016
Location: Hemlock
Posts: 42
joeojazz is an unknown quantity at this point
Re: Encoder Problems

Sorry for the lack of information.

We are trying to use encoders attached to the sonic shifter from AndyMark.

Sonic Shifter:
http://www.andymark.com/Sonic-Shifte...nicoptions.htm

And the encoder being used with the sonic shifter is the am-3132:
http://www.andymark.com/product-p/am-3132.htm

We have them wired from the encoder into the RoboRio's DIO ports. Each encoder requires two ports because they each have 2 signal channels (A and B).
Code:
package org.usfirst.frc.team5712.robot;

import com.kauailabs.navx_mxp.AHRS;
import com.ni.vision.NIVision.Image;

import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.CounterBase;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Timer;

/**
 * 
 * @author Phillip Spannagel
 *
 */
public class Robot extends IterativeRobot {

	Joystick driveStick;
	CameraServer forwardCam, reverseCam;

	VictorSP leftFront, leftRear, rightFront, rightRear, shooterR, shooterL, armLift;

	Servo servo;
	RobotDrive drive;

	SerialPort serial_port;
	
	SendableChooser autonomous;
	
	AHRS gyro;
	Encoder encoderLeft, encoderRight, encoderLift;

	double angle;
	int tickGoal = 15 * 24;
	int shootCount = 0;
	int autoLoopCounter = 0;
	
	public void robotInit() {
		driveStick = new Joystick(0);

		leftFront = new VictorSP(0);
		leftRear = new VictorSP(1);
		rightFront = new VictorSP(2);
		rightRear = new VictorSP(3);
		shooterL = new VictorSP(4);
		shooterR = new VictorSP(5);
		armLift = new VictorSP(6);
		
		servo = new Servo(7);

		drive = new RobotDrive(leftFront, leftRear, rightFront, rightRear);

		serial_port = new SerialPort(57600, SerialPort.Port.kMXP);
		byte update_rate_hz = 50;

		gyro = new AHRS(serial_port, update_rate_hz);
		gyro.resetDisplacement();

		encoderRight = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
		encoderRight.setDistancePerPulse(1);
		encoderRight.setMaxPeriod(.1);
		encoderRight.setMinRate(10);
		encoderRight.setSamplesToAverage(7);

		encoderLeft = new Encoder(2, 3, false, Encoder.EncodingType.k4X);
		encoderLeft.setDistancePerPulse(1);
		encoderLeft.setMaxPeriod(.1);
		encoderLeft.setMinRate(10);
		encoderLeft.setSamplesToAverage(7);

		encoderLift = new Encoder(4, 5, false, Encoder.EncodingType.k4X);
		encoderLift.setDistancePerPulse(1);

		forwardCam = CameraServer.getInstance();
		reverseCam = CameraServer.getInstance();

		forwardCam.setQuality(50);
		reverseCam.setQuality(50);

		forwardCam.startAutomaticCapture();
		reverseCam.startAutomaticCapture();
	}

	public void autonomousInit() {

		// drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
		// drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
		// drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
		// drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
		//start = true;
		
		gyro.zeroYaw();
		
		servo.setAngle(180);
		
		encoderLeft.reset();
		encoderRight.reset();
	}

	public void autonomousPeriodic() {
		
		angle = gyro.getYaw();
		
			
		if (autoLoopCounter < 500) {
			drive.drive(-0.3, 0.0);//start moving
			
			// gyro drive "straight"
			if (angle < -2)
			{
				rightFront.set(-0.5);
				rightRear.set(-0.5);
				
			}
			if (angle > 2) 
			{
				leftFront.set(-0.5);
				leftRear.set(-0.5);
			}
			
			}
			else if (autoLoopCounter == 500) 
			{
				drive.drive(0, 0);
				gyro.zeroYaw();
				
			}
			
			else if (autoLoopCounter < 700)
			{
					leftFront.set(-.3);
					leftRear.set(-.3);
					rightFront.set(.3);
					rightRear.set(.3);
					if(gyro.getYaw() > -145 && gyro.getYaw() < -150) {
						leftFront.set(0);
						leftRear.set(0);
						rightFront.set(0);
						rightRear.set(0);
				}
			}
			

		autoLoopCounter++;
		
		SmartDashboard.putNumber("Yaw", gyro.getYaw());
		SmartDashboard.putNumber("Servo Angle", servo.getAngle());
		/*
		 * SmartDashboard.putNumber("Count (Left)", encoderLeft.get());
		 * SmartDashboard.putNumber("Count (Right)", encoderRight.get());
		 * 
		 * SmartDashboard.putNumber("Distance (Left)",
		 * encoderLeft.getDistance()); SmartDashboard.putNumber("Raw (Left)",
		 * encoderLeft.getRaw()); SmartDashboard.putNumber("Rate (Left)",
		 * encoderLeft.getRate());
		 * 
		 * SmartDashboard.putNumber("Distance (Right)",
		 * encoderRight.getDistance()); SmartDashboard.putNumber("Raw (Right)",
		 * encoderRight.getRaw()); SmartDashboard.putNumber("Rate (Right)",
		 * encoderRight.getRate());
		 */
		SmartDashboard.putNumber("encoder (Lift)", encoderLift.get());

	}

	public void teleopPeriodic() {

		drive.arcadeDrive(driveStick);

		if (driveStick.getRawButton(1) == true) {
			drive.arcadeDrive(driveStick);
			shooterL.set(1.0);
			shooterR.set(-1.0);
			servo.setAngle(90);
			shootCount++;

			if (shootCount > 50) {
				shooterL.set(0);
				shooterR.set(0);
				servo.setAngle(180);
				shootCount = 0;
			}
		}

		if (driveStick.getRawButton(1) == false && shootCount != 0) {
			shootCount = 1;
		}

		// set servo angle
		if (driveStick.getRawButton(8) == true) {
			servo.setAngle(180);
		}
		if (driveStick.getRawButton(9) == true) {
			servo.setAngle(30);
		}

		// intake
		if (driveStick.getRawButton(2) == true) {
			shooterL.set(-.6);
			shooterR.set(.6);
		} else if (driveStick.getRawButton(2) == false) {
			shooterL.set(0);
			shooterR.set(0);
		}

		// raise and lower shooting arm
		if (driveStick.getRawButton(3) == true) {
			armLift.set(.5);
		} else if (driveStick.getRawButton(4) == true) {
			armLift.set(-.5);
		} else if (driveStick.getRawButton(3) == false) {
			armLift.set(0);
		}
	}
	public void testPeriodic() {
		/*if(start = true){
		if(angle < 180)
		{
			leftFront.set(.5);
			leftRear.set(.5);
			rightRear.set(-.5);
			rightRear.set(-0.5);
		}
		else if(angle == 180)
		{
			leftFront.set(0);
			leftRear.set(0);
			rightRear.set(0);
			rightFront.set(0);
		}
		if(angle == 180){
			start = false;
		}
	}
	if(start = false){
			gyro.resetDisplacement();
			gyro.zeroYaw();
		}
	*/
	}
}
I included all of our code. Please let me know if you need any more information.

Last edited by joeojazz : 23-02-2016 at 16:43.
Reply With Quote