-
Notifications
You must be signed in to change notification settings - Fork 614
/
Robot.java
57 lines (50 loc) · 2.04 KB
/
Robot.java
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
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj.examples.motorcontrol;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This sample program shows how to control a motor using a joystick. In the operator control part
* of the program, the joystick is read and the value is written to the motor.
*
* <p>Joystick analog values range from -1 to 1 and motor controller inputs also range from -1 to 1
* making it easy to work together.
*
* <p>In addition, the encoder value of an encoder connected to ports 0 and 1 is consistently sent
* to the Dashboard.
*/
public class Robot extends TimedRobot {
private static final int kMotorPort = 0;
private static final int kJoystickPort = 0;
private static final int kEncoderPortA = 0;
private static final int kEncoderPortB = 1;
private final PWMSparkMax m_motor;
private final Joystick m_joystick;
private final Encoder m_encoder;
/** Called once at the beginning of the robot program. */
public Robot() {
m_motor = new PWMSparkMax(kMotorPort);
m_joystick = new Joystick(kJoystickPort);
m_encoder = new Encoder(kEncoderPortA, kEncoderPortB);
// Use SetDistancePerPulse to set the multiplier for GetDistance
// This is set up assuming a 6 inch wheel with a 360 CPR encoder.
m_encoder.setDistancePerPulse((Math.PI * 6) / 360.0);
}
/*
* The RobotPeriodic function is called every control packet no matter the
* robot mode.
*/
@Override
public void robotPeriodic() {
SmartDashboard.putNumber("Encoder", m_encoder.getDistance());
}
/** The teleop periodic function is called every control packet in teleop. */
@Override
public void teleopPeriodic() {
m_motor.set(m_joystick.getY());
}
}