org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive - Code Metrics - wh1ter0se/PowerUp-2018 - Measure and Improve Code Quality continuously with Scrutinizer

org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive C
last analyzed 2018-03-21 15:27 UTC

↳ Parent: Project

Complexity

Total Complexity

Size/Duplication

Total Lines	315
Duplicated Lines	0 %

Importance

Changes	6
Bugs	0	Features	0

Metric	Value
dl	0
loc	315
rs	5.5199
c	6
b	0
f	0
wmc	56

56 Methods

Rating	Name	Size	Complexity
	ips2rpm	2	?
A	ips2rpm(double)	2	1
	initDefaultCommand	2	?
	rightIn2Mag	3	?
A	setRamps(double)	9	5
	setInverts	5	?
	rot2in	2	?
	reset	3	?
A	leftify(double)	3	4
A	rpm2ips(double)	2	1
A	PID.setPIDF(TalonSRX,Boolean,double,double,double,double)	25	1
	driveDistance	15	?
A	initDefaultCommand()	2	1
A	driveDirect(double,double)	5	5
A	PID.PID()	2	1
A	PID.getLeftInches()	2	1
	driveRLTank	17	?
A	rightMag2In(double)	2	1
	toggleReversing	2	?
A	in2rot(double)	2	1
	rpm2ips	2	?
	getLeftInches	2	?
	zeroEncoders	5	?
C	driveForza(Joystick,double,double,double)	32	11
A	driveRLTank(Joystick,double,double)	17	5
A	PID.zeroEncoders()	5	1
A	PID.getError()	2	1
A	leftMag2In(double)	2	1
A	PID.reset()	3	1
A	driveDistance(double,double)	15	1
A	rightify(double)	3	4
	driveForza	32	?
A	PID.getRightInches()	2	1
	leftMag2In	2	?
	leftIn2Mag	2	?
	getRightInches	2	?
A	PID.setPIDF(double,double,double,double)	6	1
A	rot2in(double)	2	1
A	getYAngle()	5	1
	in2rot	2	?
A	toggleReversing()	2	1
A	setDrivetrain(Drivetrain)	2	1
	driveDirect	5	?
	setDrivetrain	2	?
	getYAngle	5	?
A	leftIn2Mag(double)	2	1
A	SubsystemDrive()	31	3
A	setInverts()	5	5
	leftify	3	?
	setPIDF	6	?
	setRamps	9	?
A	rightIn2Mag(double)	3	1
	toggleDocking	3	?
	rightify	3	?
A	toggleDocking(double)	3	1
	rightMag2In	2	?

How to fix Complexity

package org.usfirst.frc.team3695.robot.subsystems;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.interfaces.Accelerometer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc.team3695.robot.Constants;
import org.usfirst.frc.team3695.robot.Robot;
import org.usfirst.frc.team3695.robot.commands.ManualCommandDrive;
import org.usfirst.frc.team3695.robot.enumeration.Bot;
import org.usfirst.frc.team3695.robot.enumeration.Drivetrain;
import org.usfirst.frc.team3695.robot.util.Xbox;

/** VROOM VROOM */
public class SubsystemDrive extends Subsystem {


    private static TalonSRX leftMaster;
    private static TalonSRX leftSlave;
    private static TalonSRX rightMaster;
    private static TalonSRX rightSlave;

    public Drivetrain drivetrain;

    public static boolean reversing;

    public static boolean docking;

    private static double dockInhibitor;

    private Accelerometer accel;

    public PID pid; // instantiate innerclass

    /* Allowable tolerance to be considered in range when driving a distance, in rotations */
    public static final double DISTANCE_ALLOWABLE_ERROR = SubsystemDrive.in2rot(2.0);

    /* runs at robot boot */
    public void initDefaultCommand() {
        setDefaultCommand(new ManualCommandDrive());
    }

    /* converts left magnetic encoder's magic units to inches */
    public static double leftMag2In(double leftMag) {
        return leftMag / 212;

    }

    /* converts right magnetic encoder's magic units to inches */
    public static double rightMag2In(double rightMag) {
        return rightMag / 212;

    }

    /* converts left magnetic encoder's magic units to inches */
    public static double leftIn2Mag(double leftMag) {
        return leftMag * 212;

    }

    /* converts right magnetic encoder's magic units to inches */
    public static double rightIn2Mag(double rightMag) {
//        return rightMag * Constants.RIGHT_MAGIC_PER_INCHES;
        return rightMag * 212;

    }
    
    /* converts RPM to inches per second */
    public static double rpm2ips(double rpm) {
        return rpm / 60.0 * Constants.WHEEL_DIAMETER * Math.PI;

    }

    /* converts an inches per second number to RPM */
    public static double ips2rpm(double ips) {
        return ips * 60.0 / Constants.WHEEL_DIAMETER / Math.PI;

    }

    /* converts rotations to distance traveled in inches */
    public static double rot2in(double rot) {
        return rot * Constants.WHEEL_DIAMETER * Math.PI;
    }

    /* converts distance traveled in inches to rotations */
    public static double in2rot(double in) {
        return in / Constants.WHEEL_DIAMETER / Math.PI;
    }

    
    

    /* apply left motor invert */
    public static final double leftify(double left) {
        Boolean invert = Robot.bot == Bot.OOF ? Constants.OOF.LEFT_MOTOR_INVERT : Constants.TEUFELSKIND.LEFT_MOTOR_INVERT;
        return left * (invert ? -1.0 : 1.0) * (docking ? dockInhibitor : 1);
    }

    /* apply right motor invert */
    public static final double rightify(double right) {
        Boolean invert = Robot.bot == Bot.OOF ? Constants.OOF.RIGHT_MOTOR_INVERT : Constants.TEUFELSKIND.RIGHT_MOTOR_INVERT;
        return right * (invert ? -1.0 : 1.0) * (docking ? dockInhibitor : 1);
    }

    
    
    /**
     * gives birth to the talons and instantiates variables (including the Bot enum)
     */
    public SubsystemDrive() {

        accel = new BuiltInAccelerometer();

        drivetrain = Drivetrain.ROCKET_LEAGUE;

        reversing = false;
        docking = false;
        dockInhibitor = 0.5d;


        pid = new PID();

        // masters
        leftMaster = new TalonSRX(Constants.LEFT_MASTER);

        	leftMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Constants.LEFT_PID, Constants.TIMEOUT_PID);
        rightMaster = new TalonSRX(Constants.RIGHT_MASTER);

        	rightMaster.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Constants.RIGHT_PID, Constants.TIMEOUT_PID);

        // slaves
        leftSlave = new TalonSRX(Constants.LEFT_SLAVE);

        	leftSlave.follow(leftMaster);
        rightSlave = new TalonSRX(Constants.RIGHT_SLAVE);

        	rightSlave.follow(rightMaster);

        switch (Robot.bot){
            case TEUFELSKIND:
            	PID.setPIDF(Constants.TEUFELSKIND.P, Constants.TEUFELSKIND.I, Constants.TEUFELSKIND.D, Constants.TEUFELSKIND.F);
                break;
            case OOF:
            	PID.setPIDF(Constants.OOF.P, Constants.OOF.I, Constants.OOF.D, Constants.OOF.F);
                break;
        }
    }

    public void setDrivetrain(Drivetrain drivetrain) {
        this.drivetrain = drivetrain;
    }

    public void toggleDocking(double dockInhibitor){
        docking = !docking;

        this.dockInhibitor = dockInhibitor;

    }

    public void toggleReversing(){
        reversing = !reversing;

    }

    public double getYAngle() {
        //http://www.hobbytronics.co.uk/accelerometer-info
        //Formula for getting the angle through the accelerometer
        //arctan returns in radians so we convert to degrees.
        return Math.atan(accel.getY() / Math.sqrt(Math.pow(accel.getX(), 2) + Math.pow(accel.getZ(), 2))) * 180 / Math.PI;

    }

    
    
    /**
     * simple rocket league drive code (not actually rocket league)
     * independent rotation and acceleration
     */
    public void driveRLTank(Joystick joy, double ramp, double inhibitor) {
        double adder = Xbox.RT(joy) - Xbox.LT(joy);
        double left = adder + (Xbox.LEFT_X(joy) / 1.333333);

        double right = adder - (Xbox.LEFT_X(joy) / 1.333333);

        left = (left > 1.0 ? 1.0 : (left < -1.0 ? -1.0 : left));
        right = (right > 1.0 ? 1.0 : (right < -1.0 ? -1.0 : right));
        setRamps(ramp);

//        if (getYAngle() > Constants.TILT_ANGLE ) {
//            leftMaster.set(ControlMode.PercentOutput, -1*Constants.RECOVERY_SPEED);
//            rightMaster.set(ControlMode.PercentOutput, -1*Constants.RECOVERY_SPEED);
//        } else if (getYAngle() < -1*Constants.TILT_ANGLE){
//            leftMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED);
//            rightMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED);
//        } else {
            leftMaster.set(ControlMode.PercentOutput, leftify(left));
            rightMaster.set(ControlMode.PercentOutput, rightify(right));
//        }
    }

    /**
     * drive code where rotation is dependent on acceleration
     * @param radius 0.00-1.00, 1 being zero radius and 0 being driving in a line
     */
    public void driveForza(Joystick joy, double ramp, double radius, double inhibitor) {
        double left = 0,
                right = 0;

        double acceleration = Xbox.RT(joy) - Xbox.LT(joy);

        setRamps(ramp);
//        if (getYAngle() > Constants.TILT_ANGLE ) {
//            leftMaster.set(ControlMode.PercentOutput, -1 * Constants.RECOVERY_SPEED);
//            rightMaster.set(ControlMode.PercentOutput, -1 * Constants.RECOVERY_SPEED);
//        } else if (getYAngle() < -1 * Constants.TILT_ANGLE){
//            leftMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED);
//            rightMaster.set(ControlMode.PercentOutput, Constants.RECOVERY_SPEED);
//        } else {

            if (!reversing ? Xbox.LEFT_X(joy) < 0 : Xbox.LEFT_X(joy) > 0) {

                right = acceleration;
                left = (acceleration * ((2 * (1 - Math.abs(Xbox.LEFT_X(joy)))) - 1)) / radius;

            } else if (!reversing ? Xbox.LEFT_X(joy) > 0 : Xbox.LEFT_X(joy) < 0) {

                left = acceleration;
                right = (acceleration * ((2 * (1 - Math.abs(Xbox.LEFT_X(joy)))) - 1)) / radius;

            } else {
                left = acceleration;
                right = acceleration;
            }
//        }][\
        left = (left > 1.0 ? 1.0 : (left < -1.0 ? -1.0 : left));
        right = (right > 1.0 ? 1.0 : (right < -1.0 ? -1.0 : right));
        leftMaster.set(ControlMode.PercentOutput, leftify(left) * inhibitor * (reversing ? -1.0 : 1.0));
        rightMaster.set(ControlMode.PercentOutput, rightify(right) * inhibitor * (reversing ? -1.0 : 1.0));

        SmartDashboard.putString("Left Master", "Left Master Voltage: " + leftMaster.getBusVoltage());
        SmartDashboard.putString("Right Master", "Right Master Voltage: " + rightMaster.getBusVoltage());
    }

    
    
    public void setRamps(double ramp) {
        if (leftMaster != null)
        	leftMaster.configOpenloopRamp(ramp, 10);

        if (leftSlave != null)
        	leftSlave.configOpenloopRamp(ramp, 10);

        if (rightMaster != null)
        	rightMaster.configOpenloopRamp(ramp, 10);

        if (rightSlave != null)
        	rightSlave.configOpenloopRamp(ramp, 10);

    }

    
    
    public boolean driveDistance(double leftIn, double rightIn) {
        double leftGoal = (leftIn2Mag(leftIn));
        double rightGoal = (rightIn2Mag(rightIn));
        leftMaster.set(ControlMode.Position, leftify(leftGoal));
    		leftSlave.follow(leftMaster);
        rightMaster.set(ControlMode.Position, rightify(rightGoal));
    		rightSlave.follow(rightMaster);

        boolean leftInRange =
                pid.getLeftInches() > leftify(leftGoal) - DISTANCE_ALLOWABLE_ERROR &&
                        pid.getLeftInches() < leftify(leftGoal) + DISTANCE_ALLOWABLE_ERROR;
        boolean rightInRange =
                pid.getRightInches() > rightify(rightGoal) - DISTANCE_ALLOWABLE_ERROR &&
                        pid.getRightInches() < rightify(rightGoal) + DISTANCE_ALLOWABLE_ERROR;
        return leftInRange && rightInRange;
    }

    public void driveDirect(double left, double right) {
        left = (left > 1.0 ? 1.0 : (left < -1.0 ? -1.0 : left));
        right = (right > 1.0 ? 1.0 : (right < -1.0 ? -1.0 : right));
        leftMaster.set(ControlMode.PercentOutput, leftify(left));
        rightMaster.set(ControlMode.PercentOutput, rightify(right));
    }

    public void setInverts() {
        rightMaster.setInverted(Robot.bot == Bot.OOF ? Constants.OOF.RIGHT_MASTER_INVERT : Constants.TEUFELSKIND.RIGHT_MASTER_INVERT);
        rightSlave.setInverted(Robot.bot == Bot.OOF ? Constants.OOF.RIGHT_SLAVE_INVERT : Constants.TEUFELSKIND.RIGHT_SLAVE_INVERT);
        leftMaster.setInverted(Robot.bot == Bot.OOF ? Constants.OOF.LEFT_MASTER_INVERT : Constants.TEUFELSKIND.LEFT_MASTER_INVERT);
        leftSlave.setInverted(Robot.bot == Bot.OOF ? Constants.OOF.LEFT_SLAVE_INVERT : Constants.TEUFELSKIND.LEFT_SLAVE_INVERT);
    }
    
    
    public static class PID {
    	Boolean enabled;
    	
    	public PID() {
    		enabled = true;
    	}
        public static void setPIDF(double p, double i, double d, double f) {
            //For future reference: Inverts must be applied individually
            setPIDF(Robot.SUB_DRIVE.leftMaster, false, p, i, d, f);
            setPIDF(Robot.SUB_DRIVE.leftSlave, false, p, i, d, f);
            setPIDF(Robot.SUB_DRIVE.rightMaster, true, p, i, d, f);
            setPIDF(Robot.SUB_DRIVE.rightSlave, true, p, i, d, f);
        }

        public static void setPIDF(TalonSRX _talon, Boolean invert, double p, double i, double d, double f) {
            /* first choose the sensor */
            _talon.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative,
                    0, Constants.TIMEOUT_PID);
            _talon.setSensorPhase(true);
            _talon.setInverted(invert);
            /* Set relevant frame periods to be at least as fast as periodic rate*/
            _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10,

                    Constants.TIMEOUT_PID);
            _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10,

                    Constants.TIMEOUT_PID);
            /* set the peak and nominal outputs */
            _talon.configNominalOutputForward(0, Constants.TIMEOUT_PID);
            _talon.configNominalOutputReverse(0, Constants.TIMEOUT_PID);
            _talon.configPeakOutputForward(1, Constants.TIMEOUT_PID);
            _talon.configPeakOutputReverse(-1, Constants.TIMEOUT_PID);
            /* set closed loop gains in slot0 - see documentation */
            _talon.selectProfileSlot(0, Constants.RIGHT_PID);
            _talon.config_kP(0, p, Constants.TIMEOUT_PID);
            _talon.config_kI(0, i, Constants.TIMEOUT_PID);
            _talon.config_kD(0, d, Constants.TIMEOUT_PID);
            _talon.config_kF(0, f, Constants.TIMEOUT_PID);
            /* set acceleration and vcruise velocity - see documentation */
            _talon.configMotionCruiseVelocity(15000, Constants.TIMEOUT_PID);

            _talon.configMotionAcceleration(6000, Constants.TIMEOUT_PID);

        }

        public void zeroEncoders() {
            Robot.SUB_DRIVE.leftMaster.setSelectedSensorPosition(0, 0, Constants.TIMEOUT_PID);
            Robot.SUB_DRIVE.rightMaster.setSelectedSensorPosition(0, 0, Constants.TIMEOUT_PID);
            Robot.SUB_DRIVE.leftMaster.setIntegralAccumulator(0,0, Constants.TIMEOUT_PID);
            Robot.SUB_DRIVE.rightMaster.setIntegralAccumulator(0,0, Constants.TIMEOUT_PID);
        }

        public double getError() {
            return (leftify(Robot.SUB_DRIVE.leftMaster.getErrorDerivative(Constants.LEFT_PID)) + rightify(Robot.SUB_DRIVE.rightMaster.getErrorDerivative(Constants.RIGHT_PID))) / 2.0;

        }

        public double getRightInches() {
            return rightMag2In(Robot.SUB_DRIVE.rightMaster.getSelectedSensorPosition(Constants.RIGHT_PID));
        }

        public double getLeftInches() {
            return leftMag2In(Robot.SUB_DRIVE.leftMaster.getSelectedSensorPosition(Constants.LEFT_PID));
        }

        public void reset() {
            Robot.SUB_DRIVE.leftMaster.setSelectedSensorPosition(0, Constants.LEFT_PID, Constants.TIMEOUT_PID);
            Robot.SUB_DRIVE.rightMaster.setSelectedSensorPosition(0, Constants.RIGHT_PID, Constants.TIMEOUT_PID);
        }
    }
}


1			package org.usfirst.frc.team3695.robot.subsystems;
2
3			import com.ctre.phoenix.motorcontrol.ControlMode;
4			import com.ctre.phoenix.motorcontrol.FeedbackDevice;
5			import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced;
6			import com.ctre.phoenix.motorcontrol.can.TalonSRX;
7			import edu.wpi.first.wpilibj.BuiltInAccelerometer;
8			import edu.wpi.first.wpilibj.Joystick;
9			import edu.wpi.first.wpilibj.command.Subsystem;
10			import edu.wpi.first.wpilibj.interfaces.Accelerometer;
11			import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
12
13			import org.usfirst.frc.team3695.robot.Constants;
14			import org.usfirst.frc.team3695.robot.Robot;
15			import org.usfirst.frc.team3695.robot.commands.ManualCommandDrive;
16			import org.usfirst.frc.team3695.robot.enumeration.Bot;
17			import org.usfirst.frc.team3695.robot.enumeration.Drivetrain;
18			import org.usfirst.frc.team3695.robot.util.Xbox;
19
20			/** VROOM VROOM */
21			public class SubsystemDrive extends Subsystem {
22
23
24			private static TalonSRX leftMaster;
25			private static TalonSRX leftSlave;
26			private static TalonSRX rightMaster;
27			private static TalonSRX rightSlave;
28
29			public Drivetrain drivetrain;
30
31			public static boolean reversing;
			0 ignored issues – show Security introduced 2018-02-23 00:10 UTC by Report Bug Copy Issue Report public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making `reversing` final. See this CWE advisory on why this is a security issue. Loading history...
32			public static boolean docking;
			0 ignored issues – show Security introduced 2018-02-23 00:10 UTC by Report Bug Copy Issue Report public static fields should always be marked final to prevent them being overwritten in unexpected ways. Consider making `docking` final. See this CWE advisory on why this is a security issue. Loading history...
33			private static double dockInhibitor;
34
35			private Accelerometer accel;
36
37			public PID pid; // instantiate innerclass
38

wh1ter0se / PowerUp-2018

org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive C last analyzed 2018-03-21 15:27 UTC

Complexity

Size/Duplication

Importance

56 Methods

How to fix Complexity

Complexity

Duplication Side-by-Side

Filter issues like

org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive C
last analyzed 2018-03-21 15:27 UTC