LineTracking2014Robot/Robot.java at master · team3238/LineTracking2014Robot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.util.MathUtils;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DigitalInput;

import java.util.Vector;

public class Robot extends IterativeRobot
{
	Joystick joystick;
	Chassis chassis;
	AnalogChannel ultrasonicSensor, gyroSensor, infaredSensor;
	DigitalInput frontPhotoswitch, rearPhotoswitch;
	DriverStationLCD driverStationLCD;
	DriverStation driverStation;

	double m_autoPConstant;
	double m_autoIConstant;
	double m_teleopPConstant;
	double m_teleopIConstant;
	double m_spinThreshold;
	double m_xPower;
	double m_yPower;

	Robot()
	{
		// Talon Ports
		final int LEFTFRONTTALONPORT = 6;
		final int LEFTREARTALONPORT = 8;
		final int RIGHTFRONTTALONPORT = 7;
		final int RIGHTREARTALONPORT = 3;
		// Digital Inputs
		final int FRONTPHOTOSWITCHPORT = 13;
		final int REARPHOTOSWITCHPORT = 14;
		// Analog Inputs
		final int ULTRASONICSENSORPORT = 1;
		final int GYROSENSORPORT = 6;
		final int INFAREDSENSORPORT = 5;
		// Driver Station Inputs
		final int JOYSTICKPORT = 1;

		chassis = new Chassis(LEFTFRONTTALONPORT, LEFTREARTALONPORT,
				RIGHTFRONTTALONPORT, RIGHTREARTALONPORT);
		joystick = new Joystick(JOYSTICKPORT);
		ultrasonicSensor = new AnalogChannel(ULTRASONICSENSORPORT);
		gyroSensor = new AnalogChannel(GYROSENSORPORT);
		frontPhotoswitch = new DigitalInput(FRONTPHOTOSWITCHPORT);
		rearPhotoswitch = new DigitalInput(REARPHOTOSWITCHPORT);
		infaredSensor = new AnalogChannel(INFAREDSENSORPORT);
		driverStationLCD = DriverStationLCD.getInstance();
		driverStation = DriverStation.getInstance();
	}

	public void robotInit()
	{
	}

	public void disabledInit()
	{
	}

	public void autonomousInit()
	{
		Vector settingsFile = FileReader.getFileContents("settings.txt");
		m_autoPConstant
				= Double.parseDouble((String) settingsFile.elementAt(0));
		m_autoIConstant
				= Double.parseDouble((String) settingsFile.elementAt(1));
		m_spinThreshold
				= Double.parseDouble((String) settingsFile.elementAt(4));
		m_xPower
				= Double.parseDouble((String) settingsFile.elementAt(5));
		m_yPower
				= Double.parseDouble((String) settingsFile.elementAt(6));
		GyroDrive.reinit();
		LineTrack.initializeConstants(m_xPower, m_yPower, m_autoPConstant,
				m_autoIConstant);
	}

	public void teleopInit()
	{
		Vector settingsFile = FileReader.getFileContents("settings.txt");
		m_teleopPConstant
				= Double.parseDouble((String) settingsFile.elementAt(2));
		m_teleopIConstant
				= Double.parseDouble((String) settingsFile.elementAt(3));
		m_spinThreshold
				= Double.parseDouble((String) settingsFile.elementAt(4));
		GyroDrive.reinit();
	}

	public void testInit()
	{
	}

	public void disabledPeriodic()
	{
		System.out.println(0.261091633
				* MathUtils.pow(infaredSensor.getVoltage(), -1.190686871));
	}

	public void autonomousPeriodic()
	{
		int gyroValue = gyroSensor.getValue() - 476;
		LineTrack.lineTrack(frontPhotoswitch, rearPhotoswitch, chassis,
				m_spinThreshold, gyroValue);
		chassis.idle();
	}

	public void teleopPeriodic()
	{
		driverStationLCD.clear();
		int gyroValue = gyroSensor.getValue() - 476;
		driverStationLCD.println(DriverStationLCD.Line.kUser1, 1,
				Integer.toString(gyroValue));
		driverStationLCD.updateLCD();
		double x = joystick.getRawAxis(1);
		double y = joystick.getRawAxis(2);
		double twist = joystick.getRawAxis(3);
		if(driverStation.getDigitalIn(2))
		{
			double adjustedRotationValue
					= GyroDrive.getAdjustedRotationValue(x, y, twist,
							m_teleopPConstant, m_teleopIConstant,
							m_spinThreshold, gyroValue);
			chassis.setJoystickData(x, y, adjustedRotationValue);
		}
		else
		{
			chassis.setJoystickData(x, y, twist);
		}
		chassis.idle();
	}

	public void testPeriodic()
	{
		System.out.println("Front Photoswitch: " + frontPhotoswitch.get());
		System.out.println("Rear Photoswitch: " + rearPhotoswitch.get());
		chassis.setJoystickData(0, 0, 0);
		chassis.idle();
	}
}