Skip to content

Commit

Permalink
Merge pull request #28 from blabel3/autofix
Browse files Browse the repository at this point in the history 
Working code for the Worlds robot
  • Loading branch information
@blabel3
blabel3 authored Apr 26, 2017
2 parents f2bd959 + 8e9b36f commit c595880
Show file tree
Hide file tree
Showing 19 changed files with 735 additions and 61 deletions.
125 changes: 111 additions & 14 deletions ftc_app-master/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlled.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,18 @@ are permitted (subject to the limitations in the disclaimer below) provided that
*/
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.teamcode.hardware.Launcher;
import org.firstinspires.ftc.teamcode.robodata.modestates.DirectionModeState;
import org.firstinspires.ftc.teamcode.robodata.modestates.IntakeModeState;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.TankDrive;
import org.firstinspires.ftc.teamcode.hardware.MotorPair;
import org.firstinspires.ftc.teamcode.robodata.AccessControl;

/**
* This file contains an example of an iterative (Non-Linear) "OpMode".
Expand All @@ -62,10 +65,19 @@ public class DriverControlled extends OpMode
/* Declare OpMode members. */
private ElapsedTime runtime = new ElapsedTime();
private Robot bot = null;
private AccessControl accessControl = new AccessControl();

private TankDrive tankDrive;
private MotorPair leftMotors;
private MotorPair rightMotors;
private Launcher launcher;

private DirectionModeState directionModeState;
private IntakeModeState modeState;

boolean doFullRotation = false;

private double power;

/*
* Code to run ONCE when the driver hits INIT
Expand All @@ -79,6 +91,13 @@ public void init() {
tankDrive = bot.getTankDrive();
leftMotors = bot.getTankDrive().getLeftMotors();
rightMotors = bot.getTankDrive().getRightMotors();
launcher = bot.getLauncher();

// modestates
modeState = new IntakeModeState();
directionModeState = new DirectionModeState();

power = 1;

telemetry.addData("Status", "Initialized");

Expand Down Expand Up @@ -106,34 +125,112 @@ public void start() {
public void loop() {
telemetry.addData("Status", "Running: " + runtime.toString());

leftMotors.setPower(gamepad1.left_stick_y);
rightMotors.setPower(gamepad1.right_stick_y);
if (accessControl.isG1Primary()) {
leftMotors.setPower(gamepad1.left_stick_y * power * directionModeState.getDirection());
rightMotors.setPower(gamepad1.right_stick_y * power * directionModeState.getDirection());
}
else if (accessControl.isG2Primary()) {
leftMotors.setPower(gamepad2.left_stick_y * power * directionModeState.getDirection());
rightMotors.setPower(gamepad2.right_stick_y * power * directionModeState.getDirection());
}
else {
leftMotors.setPower(gamepad1.left_stick_y * power * directionModeState.getDirection());
rightMotors.setPower(gamepad1.right_stick_y * power * directionModeState.getDirection());
}

//&& bot.getLauncher().getLauncherMotor().getCurrentPosition() != 0

telemetry.addData("Launcher", "Position " + launcher.getLauncherMotor().getCurrentPosition());
telemetry.addData("Left Drivetrain", "Power " + (leftMotors.getBackPower() + leftMotors.getFrontPower()) /2 );
telemetry.addData("Right Drivetrain", "Power " + (rightMotors.getBackPower() + rightMotors.getFrontPower()) /2 );
telemetry.addData("Mode State", modeState.getTelemetryStatus());
telemetry.addData("Access Control", accessControl.getTelemetryState());
telemetry.addData("Gear", directionModeState.getTelemetryMessage());

if(gamepad1.a){
bot.getColorSensor().enableLed(false);
bot.colorScan();
int[] currentRGB = bot.getRgbValues();
telemetry.addData("Red", currentRGB[0]);
telemetry.addData("Green", currentRGB[1]);
telemetry.addData("Blue", currentRGB[2]);

if(gamepad1.a || gamepad2.a){
doFullRotation = true;
bot.getLauncher().fullRotation();
}

if (gamepad1.b) {
bot.getLifter().stop();
bot.getIntake().stop();
if(doFullRotation) {
launcher.getLauncherMotor().setMode(DcMotor.RunMode.RUN_USING_ENCODER);
launcher.getLauncherMotor().setPower(1);

if (launcher.getLauncherMotor().getCurrentPosition() >= launcher.getLauncherMotor().getTargetPosition()){
launcher.getLauncherMotor().setPower(0);
doFullRotation = false;
}

}

if (gamepad1.dpad_left) {
bot.getIntake().takeInBall();
if (gamepad1.guide || gamepad2.guide) {
modeState.changeMode();
}

if (gamepad1.b || gamepad2.b) {
bot.stopMoving();
doFullRotation = false;
}

if(gamepad1.x || gamepad2.x){
power = 1;
}

if(gamepad1.y || gamepad2.y){
power = 0.5;
}

if (gamepad1.dpad_right) {
if (gamepad1.right_bumper || gamepad2.right_bumper) {
bot.getIntake().takeInBall();
} else if (gamepad1.left_bumper || gamepad2.left_bumper) {
bot.getIntake().purgeBall();
}
else if (modeState.isToggle()) {}
else {
bot.getIntake().stop();
}

if (gamepad1.dpad_up) {
bot.getLifter().ascend();
if(gamepad1.dpad_left || gamepad2.dpad_left){
bot.getSlider().setPower(1);
} else if (gamepad1.dpad_right || gamepad2.dpad_right){
bot.getSlider().setPower(-1);
} else {
bot.getSlider().setPower(0);
}

if (gamepad1.dpad_down) {
bot.getLifter().descend();
if (gamepad1.dpad_down || gamepad2.dpad_down) {
directionModeState.shiftGears();
}


// access control requesting
if (gamepad1.dpad_up || gamepad2.dpad_up) {
accessControl.changeAccess();
}

/*if (accessControl.isRequesting()) {
if (gamepad1.dpad_up || gamepad2.dpad_down) {
accessControl.setRequesting(false);
}
}
if (gamepad1.dpad_down && accessControl.isG1Primary() && accessControl.isRequesting()) {
accessControl.changeAccess();
accessControl.setRequesting(false);
}
if (gamepad2.dpad_down && accessControl.isG2Primary() && accessControl.isRequesting()) {
accessControl.changeAccess();
accessControl.setRequesting(false);
}*/

/* Recording code here */
}

Expand Down
2 changes: 1 addition & 1 deletion ftc_app-master/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Playback.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,6 @@ public interface Playback {
String INPUTS_BLUE_SAFE = "BlueTeamSafeInputs.json";
String RECENT_RUN = "RecentInputs.json";

void play();
void read();

}
53 changes: 44 additions & 9 deletions ftc_app-master/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Recording.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,20 +32,30 @@ are permitted (subject to the limitations in the disclaimer below) provided that
*/
package org.firstinspires.ftc.teamcode;

import android.content.Context;
import android.os.Handler;
import android.os.Looper;

import com.qualcomm.ftccommon.FtcRobotControllerService;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;


import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.inputtracking.Input;
import org.firstinspires.ftc.teamcode.inputtracking.InputWriter;

import org.firstinspires.ftc.robotcontroller.internal.FtcRobotControllerActivity;
import org.firstinspires.ftc.teamcode.hardware.MotorPair;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.TankDrive;
import org.firstinspires.ftc.teamcode.inputtracking.Input;

import java.io.File;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.FileOutputStream;
import java.util.ArrayList;

/**
Expand All @@ -66,6 +76,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that
public class Recording extends OpMode implements Playback
{
/* Declare OpMode members. */

private ElapsedTime runtime = new ElapsedTime();
private Robot bot = null;

Expand All @@ -74,17 +85,24 @@ public class Recording extends OpMode implements Playback
private MotorPair rightMotors;

private ArrayList<Input> inputs;
private File file;
private File file = new File(hardwareMap.appContext.getFilesDir(), Playback.INPUTS_RED);

FileOutputStream outputStream;

Looper looper;

public Looper getLooper() {
return looper;
}

/*
* Code to run ONCE when the driver hits INIT
*/
@Override
public void init() {

//file = new File(context.getFilesDir, Playback.INPUTS_RED);

bot = new Robot(hardwareMap);

inputs = new ArrayList<Input>();

tankDrive = bot.getTankDrive();
Expand Down Expand Up @@ -125,7 +143,7 @@ public void loop() {
}

if (gamepad1.b) {
bot.getLifter().stop();
//bot.getLifter().stop();
bot.getIntake().stop();
}

Expand All @@ -138,11 +156,19 @@ public void loop() {
}

if (gamepad1.dpad_up) {
bot.getLifter().ascend();
//bot.getLifter().ascend();
}

if (gamepad1.dpad_down) {
bot.getLifter().descend();
//bot.getLifter().descend();
}

if(gamepad1.left_bumper){
bot.getSlider().setPower(1);
} else if (gamepad1.right_bumper){
bot.getSlider().setPower(-1);
} else {
bot.getSlider().setPower(0);
}

inputs.add(new Input(gamepad1, runtime.time()));
Expand All @@ -155,12 +181,21 @@ public void loop() {
@Override
public void stop() {

try {
outputStream = hardwareMap.appContext.openFileOutput(Playback.INPUTS_RED, Context.MODE_PRIVATE);
InputWriter writer = new InputWriter();
writer.writeJson(outputStream, inputs);
telemetry.addData("Output", outputStream);
} catch (IOException error){
error.printStackTrace();
}

bot.stopMoving();
bot.getTankDrive().resetEncoders();

}

@Override
public void play(){}
public void read(){}

}
Loading

0 comments on commit c595880

Please sign in to comment.