Examples with Drive org.usfirst.frc948.NRGRobot2018.subsystems.Drive used on opensource projects

Example 1 with Drive

use of org.usfirst.frc948.NRGRobot2018.subsystems.Drive in project NRGRobot2018 by NRG948.

the class Robot method robotInit.

/**
 * This function is run when the robot is first started up and should be used for any
 * initialization code.
 */
@Override
public void robotInit() {
    System.out.println("robotInit() started");
    preferences = Preferences.getInstance();
    RobotMap.init();
    drive = new Drive();
    cubeAcquirer = new CubeAcquirer();
    cubeLifter = new CubeLifter();
    cubeTilter = new CubeTilter();
    climber = new Climber();
    positionTracker = new PositionTracker(0, 0);
    cubeSolenoid = new CubeSolenoid();
    OI.init();
    OI.initTriggers();
    // OI must be constructed after subsystems. If the OI creates Commands
    // (which it very likely will), subsystems are not guaranteed to be
    // constructed yet. Thus, their requires() statements may grab null
    // pointers. Bad news. Don't move it.
    initPreferences();
    RobotMap.pixy.startVisionThread();
    RobotMap.arduino.startArduinoThread();
    autoPositionChooser = new SendableChooser<AutoStartingPosition>();
    autoPositionChooser.addDefault("Left", AutoStartingPosition.LEFT);
    autoPositionChooser.addObject("Center", AutoStartingPosition.CENTER);
    autoPositionChooser.addObject("Right", AutoStartingPosition.RIGHT);
    autoMovementChooser = new SendableChooser<AutoMovement>();
    autoMovementChooser.addObject("Switch", AutoMovement.SWITCH);
    autoMovementChooser.addObject("Scale", AutoMovement.SCALE);
    autoMovementChooser.addDefault("Both", AutoMovement.BOTH);
    autoMovementChooser.addDefault("Forward", AutoMovement.FORWARD);
    autoMovementChooser.addDefault("None", AutoMovement.NONE);
    SmartDashboard.putData("Choose autonomous position", autoPositionChooser);
    SmartDashboard.putData("Choose autonomous movement", autoMovementChooser);
    System.out.println("robotInit() done");
}

Example 2 with Drive

use of org.usfirst.frc948.NRGRobot2018.subsystems.Drive in project Relic_Main by TeamOverdrive.

the class TestGyro method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    Drive mDrive = new Drive();
    mDrive.init(this, true);
    telemetry.addData("wait", "");
    telemetry.update();
    waitForStart();
    while (opModeIsActive()) {
        telemetry.addData("Gyro Left", mDrive.getGyroAngleDegrees());
        telemetry.update();
    }
}

Example 3 with Drive

use of org.usfirst.frc948.NRGRobot2018.subsystems.Drive in project Relic_Main by TeamOverdrive.

the class TestingSplinesDoubleTrajectory method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    // Used to log the speed and position to be able to plot the data in Excel
    PhoneLogger logger = new PhoneLogger("SpeedPosTimeData.csv");
    // Generate a simple path for testing
    Path path = Line.calculate(defaultTrajectoryConfig, TrajectoryGenerator.SCurvesStrategy, 0, 0, 15, 0);
    // Init our drivetrain
    Drive mDrive = new Drive();
    mDrive.init(this, true);
    // Use speed control to make the output of the motor linear
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    /*
        * Used to make it easier to save constants
        * P term, D term, V term, A term, P term for gyro compensation
        * We are not using I because the feed-forward + feed-back control
        * will be enough to control the system
        */
    FollowerConfig followerConfig = new FollowerConfig(Constants.p.getDouble(), Constants.d.getDouble(), Constants.v.getDouble(), Constants.a.getDouble(), Constants.headingP.getDouble());
    // New Drive Controller
    TrajectoryDriveController controller = new TrajectoryDriveController(mDrive, followerConfig);
    // Tell the controller what profiles to follow
    // (the two 1's are in case we want to mirror the path)
    controller.loadPath(path, 1, 1);
    // Lets go
    telemetry.setAutoClear(false);
    telemetry.addData("Ready to Start", "");
    telemetry.update();
    waitForStart();
    telemetry.setAutoClear(true);
    telemetry.clearAll();
    // Used to calculate the change in time from the last reading
    ElapsedTime dtTimer = new ElapsedTime();
    ElapsedTime timeToComplete = new ElapsedTime();
    double totalTime = 0;
    boolean first = true;
    // Used to calculate speed
    double lastPosition = 0;
    while (opModeIsActive()) {
        // data
        telemetry.addData("Time To Complete", totalTime);
        telemetry.addData("Left Distance", mDrive.getLeftDistanceInches());
        telemetry.addData("Right Distance", mDrive.getRightDistanceInches());
        telemetry.addData("Left Speed", controller.wantedLeftSpeed);
        telemetry.addData("Right Speed", controller.wantedRightSpeed);
        telemetry.addData("Speed output", controller.getGoal());
        telemetry.addData("Gyro", mDrive.getGyroAngleRadians());
        telemetry.addData("dt", dtTimer.seconds());
        telemetry.addData("Number of Segments", controller.getNumSegments());
        telemetry.addData("Current Pos", controller.getFollowerCurrentSegment());
        telemetry.update();
        // Make sure we only update when we told the controller/trajectory we would
        if (dtTimer.seconds() > defaultTrajectoryConfig.dt && !controller.isOnTarget()) {
            // Update our controller
            controller.update();
            // Used to log data so we can plot it
            double currentTime = timeToComplete.milliseconds();
            double currentLeftPos = mDrive.getLeftDistanceInches();
            double left_speed = (currentLeftPos - lastPosition) / dtTimer.milliseconds();
            logger.write(String.valueOf(currentTime) + "," + String.valueOf(path.getLeftWheelTrajectory().getSegment(controller.getFollowerCurrentSegment()).pos) + "," + String.valueOf(currentLeftPos));
            idle();
            dtTimer.reset();
        } else if (controller.isOnTarget()) {
            mDrive.setLeftRightPower(0, 0);
            if (first) {
                // Track how long it took to complete the path
                totalTime = timeToComplete.seconds();
                first = false;
            }
        }
    }
    // Close the logger
    logger.close();
    // Be able to move the robot after we run it
    mDrive.setRunMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}

Example 4 with Drive

use of org.usfirst.frc948.NRGRobot2018.subsystems.Drive in project Relic_Main by TeamOverdrive.

the class TestFollowerWheel method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    Drive mDrive = new Drive();
    mDrive.init(this, true);
    mDrive.zeroSensors();
    FollowerWheel mFollow = new FollowerWheel();
    mFollow.init(this, false);
    FollowerDrive followerDrive = new FollowerDrive(mDrive, mFollow);
    telemetry.addData("Hello", "");
    telemetry.update();
    waitForStart();
    ElapsedTime t = new ElapsedTime();
    mDrive.setRunMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    mDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
    while (opModeIsActive()) {
        followerDrive.update();
        telemetry.addData("X", followerDrive.getX());
        telemetry.addData("Y", followerDrive.getY());
        telemetry.addData("Radius", followerDrive.radius());
        telemetry.addData("Angle", followerDrive.angle());
        telemetry.addData("Real Angle", mDrive.getGyroAngleRadians());
        telemetry.update();
    }
    mDrive.setLeftRightPower(0, 0);
    telemetry.addData("X", followerDrive.getX());
    telemetry.addData("Y", followerDrive.getY());
    telemetry.update();
    while (opModeIsActive()) Thread.yield();
}

Example 5 with Drive

use of org.usfirst.frc948.NRGRobot2018.subsystems.Drive in project Relic_Main by TeamOverdrive.

the class TestingSplinesSingleTrajectory method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    telemetry.addData("Started", "");
    telemetry.update();
    TrajectoryGenerator.Config config = new TrajectoryGenerator.Config();
    // In/s
    config.max_vel = 2.0 * 12;
    // In/s^2
    config.max_acc = 0.6 * 12;
    // In/s^3
    config.max_jerk = 0.4 * 12;
    // seconds
    config.dt = 0.01;
    Trajectory trajectory = TrajectoryGenerator.generate(config, TrajectoryGenerator.SCurvesStrategy, 0, 0, 10, 0, 0);
    telemetry.addData("Trajectory Generation Finished", "");
    telemetry.update();
    double gearing = Constants.DRIVE_GEAR_REDUCTION;
    // Volts
    double Vmax = 12;
    // lbs
    double mass = 48.0 / 9.8;
    double numberOfMotors = 2;
    // lb-in
    double stallTorque = 1.531179;
    double velocityMax = (5400 * Math.PI * Constants.WHEEL_DIAMETER_INCHES) / (gearing);
    double kv = Vmax / velocityMax;
    double accelerationMax = (2 * numberOfMotors * stallTorque * gearing) / (4.0 * mass);
    double ka = Vmax / accelerationMax;
    FollowerConfig followerConfig = new FollowerConfig(0.1, 0, kv, ka, Math.PI / 1000);
    TrajectoryFollower follower = new TrajectoryFollower("Follower");
    follower.configure(followerConfig.get()[0], followerConfig.get()[1], followerConfig.get()[2], followerConfig.get()[3], followerConfig.get()[4]);
    follower.setTrajectory(trajectory);
    Drive mDrive = new Drive();
    mDrive.init(this, true);
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    waitForStart();
    ElapsedTime time = new ElapsedTime();
    sleep(10);
    while (!follower.isFinishedTrajectory() && opModeIsActive()) {
        if (time.seconds() > config.dt) {
            double distance = (mDrive.getRightDistanceInches() + mDrive.getLeftDistanceInches()) / 2;
            double forwardSpeed = follower.calculate(distance);
            telemetry.addData("Distance", distance);
            telemetry.addData("Turning Error", mDrive.getGyroAngleDegrees() - follower.getHeading());
            telemetry.addData("Forward", forwardSpeed);
            telemetry.update();
            mDrive.setLeftRightPower(forwardSpeed, forwardSpeed);
            time.reset();
        }
    }
    mDrive.setLeftRightPower(0, 0);
}