Go to Post There is a very fine line between passion and emotion, sometime it gets crossed. Let's not confuse the two. - JohnBoucher [more]
Home
Go Back   Chief Delphi > Technical > Programming > Java
CD-Media   CD-Spy  
portal register members calendar search Today's Posts Mark Forums Read FAQ rules

 
 
 
Thread Tools Rate Thread Display Modes
Prev Previous Post   Next Post Next
  #1   Spotlight this post!  
Unread 15-11-2015, 20:35
Mr.Roboto3335's Avatar
Mr.Roboto3335 Mr.Roboto3335 is offline
Wait, What?
AKA: Jimmy
FRC #3335 (Cy-Borgs)
Team Role: Programmer
 
Join Date: Nov 2011
Rookie Year: 2011
Location: Texas
Posts: 47
Mr.Roboto3335 is an unknown quantity at this point
Old Code

Just posing old code for anybody to use and view.

Code:
package org.usfirst.frc.team3335.robot;

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*                                                                            */
/*                    Written by The Lost King and his 25 slaves              */
/*----------------------------------------------------------------------------*/

//import com.ni.vision.NIVision;
//import com.ni.vision.NIVision.Image;
//import com.ni.vision.NIVision.ImageType;
//import edu.java.ArtificialIntelligence;
//import edu.hell.SoulsOfTheDamned;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.Joystick.AxisType;
import edu.wpi.first.wpilibj.Joystick.RumbleType;
//import edu.wpi.first.wpilibj.Joystick.AxisType;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class Robot extends IterativeRobot {

	private static final double maxSpeed = 0.6;
	//private static final double ultraInchesPerVolt = 106.1391; // Circuit board
																// = 0 in.

	// Ultrasonic range is 7 in. to 76 in.

	private double autoTimeStart;
	private Compressor compressor;
	private Relay relay;
	private Solenoid solenoidOut;
	private Solenoid solenoidIn;
	private Joystick controller;
	private RobotDrive myRobot;
	private SpeedController mcBackLeft, mcFrontLeft, mcBackRight, mcFrontRight,
			mcLift1, mcLift2;
	//private BuiltInAccelerometer accel;
	//private Gyro gyro;
	// private CameraServer server;
	//private Image image;
	//private int niSession;

	private DigitalInput limit, limit2;
	//private AnalogInput ultrasonic;
	//private AnalogInput gyroTest;
	//private AnalogInput pressure;

	private boolean hasExtended;
	//private boolean useGyro;

	double Kp = 0.03;

	public void robotInit() {
		joystickInit();
		startCompressor();
		sensorInit();
		speedControllerInit();
		robotDriveInit();
		// cameraInit();
		//accel = new BuiltInAccelerometer();
		autoTimeStart = 0;
	}

	public void joystickInit() {
		controller = new Joystick(0);
	}

	public void sensorInit() {
		limit = new DigitalInput(0);
		limit2 = new DigitalInput(1);
		//ultrasonic = new AnalogInput(2);
		//gyro = new Gyro(new AnalogInput(0));
		//gyroTest = new AnalogInput(1);
		//pressure = new AnalogInput(3);

		//useGyro = true; // Manually set this variable to true to use gyro.
	}

	public void speedControllerInit() {
		mcBackRight = new Victor(0);
		mcFrontRight = new Victor(1);
		mcBackLeft = new Victor(2);
		mcFrontLeft = new Victor(3);
		mcLift1 = new Talon(4);
		mcLift2 = new Talon(5);
	}

	public void robotDriveInit() {
		myRobot = new RobotDrive(mcBackLeft, mcFrontLeft, mcBackRight,
				mcFrontRight);
	}

	public void startCompressor() {
		solenoidIn = new Solenoid(0);
		solenoidOut = new Solenoid(1); // Pneumatics
		relay = new Relay(0);
		relay.set(Relay.Value.kOff);
		compressor = new Compressor(2);
	}

	 /*public void cameraInit() {
		 server = CameraServer.getInstance();
	server.setQuality(50);
	 image = NIVision.imaqCreateImage(NIVision.ImageType.IMAGE_RGB, 0);
	 niSession = NIVision.IMAQdxOpenCamera("cam0",
	 NIVision.IMAQdxCameraControlMode.CameraControlModeController);
	 NIVision.IMAQdxConfigureGrab(niSession);
	 }*/
	public void autonomousInit() {
		autoTimeStart = Timer.getFPGATimestamp();
		//gyro.reset();
	}

	public void autonomousPeriodic() {
		//double angle = gyro.getAngle();
		double autoTimeCurr = Timer.getFPGATimestamp() - autoTimeStart;
		if (autoTimeCurr <= 0.5) {
			solenoidIn.set(true);
		} else if (autoTimeCurr <= 2) {
			mcLift1.set(0.7);
		} else if (autoTimeCurr <= 4) {
			//myRobot.drive(-1.0, -angle * Kp);
		}

		SmartDashboard.putString("DB/String 1",
				String.format("%.3f", autoTimeCurr));

		// SUPER DUPER AUTONOMOUS
	}

	public void teleopInit() {
		 //NIVision.IMAQdxStartAcquisition(niSession);
		//gyro.reset();
	}

	public void teleopPeriodic() {
		if (controller.getRawButton(9)) {
			controller.setRumble(RumbleType.kLeftRumble, 1);
			controller.setRumble(RumbleType.kRightRumble, 1);
			//gyro.reset();
		} else {
			controller.setRumble(RumbleType.kLeftRumble, 0);
			controller.setRumble(RumbleType.kRightRumble, 0);
		}
		// NIVision.IMAQdxGrab(niSession, image, 1);
		// server.setImage(image);

		double x = getDeadZone(controller.getAxis(Joystick.AxisType.kX), 0.15);
		double y = getDeadZone(controller.getAxis(Joystick.AxisType.kY), 0.15);
		double r = getDeadZone(controller.getRawAxis(4) * 1, 0.15);

		 mcFrontLeft.set(normalize(x - y + r) * maxSpeed);
		 mcBackRight.set(normalize(-x + y + r) * maxSpeed);
		 mcBackLeft.set(normalize(-x - y + r) * maxSpeed);
		 mcFrontRight.set(normalize(x + y + r) * maxSpeed);

		//double joystickAngle = Math.toDegrees(-Math.atan2(-y, x)) + 90;
		//joystickAngle = (joystickAngle % 360 + 360) % 360;

		//if (useGyro) {
			//mcFrontLeft.set(-normalize(Math.hypot(-y, x)
				//	* joystickFunction(joystickAngle - gyro.getAngle()) + r)
					//* maxSpeed);
			//mcFrontRight.set(-normalize(Math.hypot(-y, x)
					//* joystickFunction(-joystickAngle + gyro.getAngle()) - r)
					//* maxSpeed);
			//mcBackLeft.set(normalize(Math.hypot(-y, x)
					//* joystickFunction(-joystickAngle + gyro.getAngle()) + r)
					//* maxSpeed);
			//mcBackRight.set(-normalize(Math.hypot(-y, x)
					//* joystickFunction(joystickAngle - gyro.getAngle()) - r)
					//* maxSpeed);
		//} else {
			//mcFrontLeft.set(-normalize(Math.hypot(-y, x)
				//* joystickFunction(joystickAngle) + r)
					//* maxSpeed);
			//mcFrontRight.set(-normalize(Math.hypot(-y, x)
					//* joystickFunction(-joystickAngle) - r)
				//	* maxSpeed);
			//mcBackLeft.set(normalize(Math.hypot(-y, x)
					//* joystickFunction(-joystickAngle) + r)
				//	* maxSpeed);
			//mcBackRight.set(-normalize(Math.hypot(-y, x)
				//	* joystickFunction(joystickAngle) - r)
			//		* maxSpeed);
		//}

		//if (pressure.getVoltage() < 3) {
			//relay.set(Relay.Value.kReverse);
		//} else {
			//relay.set(Relay.Value.kOff);
		//}

		if (controller.getRawButton(8)) {
			relay.set(Relay.Value.kReverse);
		} else if (controller.getRawButton(7)) {
			relay.set(Relay.Value.kOff);
		}

		// Pneumatics:
		if (controller.getRawButton(2) && hasExtended) {
			solenoidOut.set(true);
			solenoidIn.set(false);

			hasExtended = false;
		} else if (controller.getRawButton(1) && !hasExtended) {
			solenoidOut.set(false);
			solenoidIn.set(true);

			hasExtended = true;
		} else {
			solenoidOut.set(false);
			solenoidIn.set(false);
		}

		if (controller.getRawButton(5) && controller.getRawButton(6)) {
			mcLift1.set(0);
			mcLift2.set(0);
		} else if (controller.getRawButton(6) && limit.get() == false) {
			relay.set(Relay.Value.kOff);
			mcLift1.set(0.7);
			mcLift2.set(0.7);
		} else if (controller.getRawButton(5) && limit.get() == false) {
			relay.set(Relay.Value.kOff);
			mcLift1.set(-0.4);
			mcLift2.set(-0.4);
		} else {
			if (controller.getRawButton(8)) {
				relay.set(Relay.Value.kReverse);
			} else if (controller.getRawButton(7)) {
				relay.set(Relay.Value.kOff);
			}
			mcLift1.set(0);
			mcLift2.set(0);
			
		}
		//SmartDashboard
			//	.putString(
				//		"DB/String 0",
					//	String.format("Ultrasonic Volt: %.4f",
				//				ultrasonic.getVoltage()));
		SmartDashboard.putString("DB/String 1",
				String.format("D-Pad: %d", controller.getPOV()));
		SmartDashboard.putString("DB/String 2",
				String.format("Axis X: %.4f", controller.getAxis(AxisType.kX)));
		SmartDashboard.putString("DB/String 3",
				String.format("Axis Y: %.4f", controller.getAxis(AxisType.kY)));
		SmartDashboard.putString("DB/String 4",
				String.format("Turn Axis: %.4f", controller.getRawAxis(4)));
		//SmartDashboard.putString(
			//	"DB/String 5",
				//String.format("Distance: %.4f", ultrasonic.getVoltage()
					//	* ultraInchesPerVolt));
//		SmartDashboard.putString("DB/String 6",
	//			String.format("Gyro: %.4f", gyro.getAngle()));
		//SmartDashboard.putString("DB/String 7",
			//	String.format("Gyro Rate: %.4f", gyro.getRate()));
	//	SmartDashboard.putString("DB/String 8",
		//		String.format("Gyro Test: %.4f", gyroTest.getVoltage()));
		//SmartDashboard.putString("DB/String 9", 
			//	String.format("Pressure: %.4f", pressure.getVoltage()));
	//SmartDashboard.putString("DB/String 8",
		//		String.format("%.3f", joystickAngle));
	}

	//
	public void disabledInit() {
		// NIVision.IMAQdxStopAcquisition(niSession);
		SmartDashboard.putString("DB/String 0", "");
		SmartDashboard.putString("DB/String 1", "");
		SmartDashboard.putString("DB/String 2", "");
		SmartDashboard.putString("DB/String 3", "");
		SmartDashboard.putString("DB/String 4", "");
		SmartDashboard.putString("DB/String 5", "");
		SmartDashboard.putString("DB/String 6", "");
		SmartDashboard.putString("DB/String 7", "");
		SmartDashboard.putString("DB/String 8", "");
		SmartDashboard.putString("DB/String 9", "");
		solenoidIn.set(false);
		solenoidOut.set(false);
	}

	public void testPeriodic() {

		SmartDashboard.putString("DB/String 2",
				String.format("Axis X: %.4f", controller.getAxis(AxisType.kX)));
		SmartDashboard.putString("DB/String 3",
				String.format("Axis Y: %.4f", controller.getAxis(AxisType.kY)));
		SmartDashboard.putString("DB/String 4",
				String.format("Turn Axis: %.4f", controller.getRawAxis(4)));

		double x = getDeadZone(controller.getAxis(Joystick.AxisType.kX), 0.15);
		double y = getDeadZone(controller.getAxis(Joystick.AxisType.kY), 0.15);

		double joystickAngle = Math.toDegrees(-Math.atan2(-y, x)) + 90;
		SmartDashboard.putString("DB/String 8",
				String.format("%.3f", joystickAngle));
	}

	public double getDeadZone(double axis, double zone) {
		return Math.abs(axis) > zone ? axis : 0;
	}

	public double joystickFunction(double angle) {
		angle = (angle % 360 + 360) % 360;
		if (0 <= angle && angle < 90)
			return 1;
		else if (90 <= angle && angle < 180)
			return -Math.cos(Math.toRadians(2 * angle));
		else if (180 <= angle && angle < 270)
			return -1;
		else
			return Math.cos(Math.toRadians(2 * angle));
	}

	public double normalize(double value) {
		return Math.min(1, Math.max(-1, value));
	}
}
__________________
Wait, what?
Reply With Quote
 


Thread Tools
Display Modes Rate This Thread
Rate This Thread:

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

vB code is On
Smilies are On
[IMG] code is On
HTML code is Off
Forum Jump


All times are GMT -5. The time now is 08:42.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.


Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi