2016Robot/Shooter.java at master · hems03/2016Robot · 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
package org.usfirst.frc.team5624.robot.subsystems;

import org.usfirst.frc.team5624.robot.PIDSpeedController;
import org.usfirst.frc.team5624.robot.Robot;
import org.usfirst.frc.team5624.robot.RobotMap;
import org.usfirst.frc.team5624.robot.commands.Idle;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 */
public class Shooter extends Subsystem  {
	Victor shooterMotor;
	public Encoder shooterEncoder;
	public PIDSpeedController shooterControl;
	double inchesPerPulse=Math.PI*2*2/20;
	public Shooter(){


		shooterMotor= new Victor(RobotMap.shooterIn);


		shooterEncoder = new Encoder(RobotMap.ShooterEncoderPow,RobotMap.ShooterEncoderSignal);
		shooterEncoder.setDistancePerPulse(inchesPerPulse);
		shooterEncoder.setSamplesToAverage(5);
		shooterEncoder.setPIDSourceType(PIDSourceType.kRate);
		shooterControl=new PIDSpeedController(shooterEncoder,shooterMotor,"Shooter","Wheel");


	}

    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public void initDefaultCommand() {
    	setDefaultCommand(new Idle());

        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    public void set(double val){
    	shooterControl.set(val);


    }
    public void updatePID(){
    	double P=Robot.prefs.getDouble("P", 0);
    	double I=Robot.prefs.getDouble("I", 0);
    	double D=Robot.prefs.getDouble("D", 0);
    	double F=Robot.prefs.getDouble("F", 0);
    	shooterControl.setConstants(P,I,D,F);


    }

    /*public void clearPID(){
    	shooterControl.setConstants(0, 0, 0,0);
    }
    */
    public void log(){
    	SmartDashboard.putData(getCurrentCommand());
    	SmartDashboard.putNumber("ShooterSpeed", shooterEncoder.getRate());
    	SmartDashboard.putNumber("Speed Setpoint", shooterControl.getSetpoint());


    }


	}