Permalink
Browse files

Fixed shit

Good shit is fixed
  • Loading branch information...
williamlin1 committed Jan 13, 2018
1 parent 873fbf7 commit 083dbec63013f77d07d456d121e6ecca1ae40f92
@@ -21,7 +21,6 @@
public RobotComp Comp;
public Servo LAST;
public Servo RAST;
@@ -46,6 +45,8 @@
public void init(HardwareMap Map){
Comp = new RobotComp();
LFMotor = Map.dcMotor.get("LF");
RFMotor = Map.dcMotor.get("RF");
LBMotor = Map.dcMotor.get("LB");
@@ -111,8 +112,8 @@ public void stop(){
public void turn(Direction direction, int degrees, MotorGroup Right, MotorGroup Left){
Right.resetEncoders();
Left.resetEncoders();
int LeftDistance = Comp.computeTurningPos(direction, degrees,Direction.LEFT, 19.0, Version.TWO);
int RightDistance = Comp.computeTurningPos(direction, degrees,Direction.RIGHT, 19.0, Version.TWO);
int LeftDistance = Comp.computeTurningPos(direction, degrees, Direction.LEFT, 26.5, Version.TWO);
int RightDistance = Comp.computeTurningPos(direction, degrees, Direction.RIGHT, 26.5, Version.TWO);
Left.setTargetPosition(LeftDistance);
Right.setTargetPosition(RightDistance);
Left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@@ -133,16 +134,18 @@ public void driveDir(Direction direction, double distance, MotorGroup Right, Mot
Left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
switch(direction){
case FORWARDS:
Right.setTargetPosition(Comp.computePositionD(RobotMath.toMeters(distance),Version.TWO));
Left.setTargetPosition(Comp.computePositionD(RobotMath.toMeters(distance), Version.TWO));
int pos = -Comp.computePositionD(RobotMath.toMeters(distance), Version.TWO);
Right.setTargetPosition(pos);
Left.setTargetPosition(pos);
break;
case BACKWARDS:
Right.setTargetPosition(-Comp.computePositionD(RobotMath.toMeters(distance), Version.TWO));
Left.setTargetPosition(-Comp.computePositionD(RobotMath.toMeters(distance), Version.TWO));
pos = Comp.computePositionD(RobotMath.toMeters(distance), Version.TWO);
Right.setTargetPosition(pos);
Left.setTargetPosition(pos);
break;
}
Right.setPowers(0.3);
Left.setPowers(0.3);
Right.setPowers(0.5);
Left.setPowers(0.5);
while(Right.isBusy()&&Left.isBusy()){}
Right.setPowers(0);
Left.setPowers(0);
@@ -2,10 +2,17 @@
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.vuforia.HINT;
import com.vuforia.Vuforia;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import Team7159.Enums.Colors;
import Team7159.Enums.Direction;
import Team7159.Enums.Side;
import Team7159.FBarRobot;
import Team7159.Utils.ColorManip;
import Team7159.Utils.MotorGroup;
@@ -21,27 +28,48 @@
FBarRobot robot = new FBarRobot();
ColorManip color;
VuforiaLocalizer vuforia;
VuforiaLocalizer.Parameters parameters;
private MotorGroup Left = new MotorGroup();
private MotorGroup Right = new MotorGroup();
@Override
public void runOpMode() throws InterruptedException {
robot.init(hardwareMap);
Left.addMotor(robot.LFMotor,robot.LBMotor);
Right.addMotor(robot.RFMotor,robot.RBMotor);
robot.init(hardwareMap);
robot.AAST.setPosition(0);
robot.AASB.setPosition(0.4);
robot.colorSensor.enableLed(true);
parameters = new VuforiaLocalizer.Parameters(com.qualcomm.ftcrobotcontroller.R.id.cameraMonitorViewId);
parameters.vuforiaLicenseKey = "AVktbtv/////AAAAGY1koTqeTUyKsH17S4sxg5FdzjlL4sab4r1TteImHLQZaxsQP96TVimg0LSECJMSTY" +
"/hMmyl4Ko8WqEFHdESFWl5CNgqDIkVJsLD4ivpj1OAwtHu6z1Me1lnhV/DlBshYL9nsfqWCvVyPPpMkYBj3DRGGI" +
"6OHwD29CokKIxnknH8sV/k8xdVFSAmsRqBney+t4+c7vmUw39q7qrsE63Pf6wnFxYLkDz4uFvjy3IHbX3/OLojTN" +
"Gk4/sHOWnME0c8EEVXUZAoXPM/7jJK/ksBrYMPyJTZOeMPhcTMtjsPNMVx54p5yICLcIGjqPwTih1Z88RGDGKIuY" +
"vIrnSMjUnJNZtshuuqadeAXk2HyGS6DR3K";
parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
parameters.cameraMonitorFeedback = VuforiaLocalizer.Parameters.CameraMonitorFeedback.AXES;
vuforia = ClassFactory.createVuforiaLocalizer(parameters);
vuforia.setFrameQueueCapacity(6);
Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 4);
VuforiaTrackables Images = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
VuforiaTrackable template = Images.get(0);
waitForStart();
Images.activate();
//swing servo down
robot.AAST.setPosition(0.8);
Thread.sleep(2000);
Thread.sleep(1000);
//0.5 is placeholder for straight down as far as possible
int r = robot.colorSensor.red();
@@ -51,46 +79,62 @@ public void runOpMode() throws InterruptedException {
telemetry.addData("blue ", b);
telemetry.update();
Colors backColor = r>b?Colors.RED:Colors.BLUE;
telemetry.addData("num left ", Left.number());
telemetry.addData("num right ", Right.number());
telemetry.update();
Colors backColor = r>b? Colors.RED: Colors.BLUE;
if(r==b){
telemetry.addData(" r and b are the same", "a");
telemetry.update();
}else if(backColor.equals(Colors.RED)){
//Hit back, placeholder value
robot.AASB.setPosition(0.3);
robot.AASB.setPosition(1.0);
}else{
//Hit foward, placeholder value
robot.AASB.setPosition(0.8);
robot.AASB.setPosition(0.4);
}
robot.AAST.setPosition(0);
robot.colorSensor.enableLed(false);
//back as far as possible
// robot.SAB.setPosition(1);
//Resetting top to back
// robot.SAT.setPosition(1);
RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(template);
robot.AAST.setPosition(0);
if(vuMark.equals(RelicRecoveryVuMark.CENTER)){
telemetry.addData("vuMark ", "In the center");
telemetry.update();
//VUFORIOS THIS SHIT DUDE
robot.driveDir(Direction.FORWARDS,36, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
//do some center stuff
}else if(vuMark.equals(RelicRecoveryVuMark.LEFT)){
telemetry.addData("vuMark ", "In the left");
telemetry.update();
robot.driveDir(Direction.BACKWARDS,35,Right,Left);
robot.turn(Direction.RIGHT, 90, Right,Left);
robot.driveDir(Direction.FORWARDS,28, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
// do some left stuff
}else if(vuMark.equals(RelicRecoveryVuMark.RIGHT)){
telemetry.addData("vuMark ", "In the right");
telemetry.update();
//VUFORIA STUFF THAT IM NOT DOING RIGHT NOW
robot.driveDir(Direction.FORWARDS,45, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
Side side = Side.CENTER;
// Side side is wahtever side its suppsdeo to be on
// do some right stuff
}else{
telemetry.addData("vuMark ", "who nose");
telemetry.update();
if(side.equals(Side.CENTER)){
//go put it in the center
}else if(side.equals(Side.LEFT)){
//go to teh left
}else if(side.equals(Side.RIGHT)){
//go to the right
}else if(side.equals(Side.NONEFOUND)){
//stop
}
robot.driveDir(Direction.LEFT,36, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
//it's unknown, just go to the center i suppose
}
}
}
@@ -2,12 +2,18 @@
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.vuforia.HINT;
import com.vuforia.Vuforia;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.navigation.RelicRecoveryVuMark;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
import Team7159.Enums.Colors;
import Team7159.Enums.Direction;
import Team7159.Enums.Side;
import Team7159.FBarRobot;
import Team7159.Utils.ColorManip;
import Team7159.Utils.MotorGroup;
/**
@@ -20,69 +26,119 @@
FBarRobot robot = new FBarRobot();
VuforiaLocalizer vuforia;
VuforiaLocalizer.Parameters parameters;
MotorGroup Left = new MotorGroup();
MotorGroup Right = new MotorGroup();
@Override
public void runOpMode() throws InterruptedException {
robot.init(hardwareMap);
Right.addMotor(robot.RFMotor,robot.RBMotor);
Left.addMotor(robot.LFMotor,robot.LBMotor);
Right.addMotor(robot.RFMotor,robot.RBMotor);
robot.AAST.setPosition(0);
robot.AASB.setPosition(0.4);
robot.colorSensor.enableLed(true);
parameters = new VuforiaLocalizer.Parameters(com.qualcomm.ftcrobotcontroller.R.id.cameraMonitorViewId);
parameters.vuforiaLicenseKey = "AVktbtv/////AAAAGY1koTqeTUyKsH17S4sxg5FdzjlL4sab4r1TteImHLQZaxsQP96TVimg0LSECJMSTY" +
"/hMmyl4Ko8WqEFHdESFWl5CNgqDIkVJsLD4ivpj1OAwtHu6z1Me1lnhV/DlBshYL9nsfqWCvVyPPpMkYBj3DRGGI" +
"6OHwD29CokKIxnknH8sV/k8xdVFSAmsRqBney+t4+c7vmUw39q7qrsE63Pf6wnFxYLkDz4uFvjy3IHbX3/OLojTN" +
"Gk4/sHOWnME0c8EEVXUZAoXPM/7jJK/ksBrYMPyJTZOeMPhcTMtjsPNMVx54p5yICLcIGjqPwTih1Z88RGDGKIuY" +
"vIrnSMjUnJNZtshuuqadeAXk2HyGS6DR3K";
parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
parameters.cameraMonitorFeedback = VuforiaLocalizer.Parameters.CameraMonitorFeedback.AXES;
vuforia = ClassFactory.createVuforiaLocalizer(parameters);
vuforia.setFrameQueueCapacity(6);
Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 4);
VuforiaTrackables Images = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
VuforiaTrackable template = Images.get(0);
waitForStart();
Images.activate();
//swing servo down
robot.AAST.setPosition(0.8);
Thread.sleep(2000);
Thread.sleep(1000);
//0.5 is placeholder for straight down as far as possible
int r = robot.colorSensor.red();
int b = robot.colorSensor.blue();
telemetry.addData("red ", r);
telemetry.addData("blue ", b);
telemetry.update();
Colors frontC = r>b?Colors.RED:Colors.BLUE;
telemetry.addData("num left ", Left.number());
telemetry.addData("num right ", Right.number());
telemetry.update();
Colors backColor = r>b? Colors.RED: Colors.BLUE;
if(r==b){
telemetry.addData(" r and b are the same", "a");
telemetry.update();
}else if(frontC.equals(Colors.RED)){
}else if(backColor.equals(Colors.RED)){
//Hit back, placeholder value
robot.AASB.setPosition(0.3);
robot.AASB.setPosition(1.0);
}else{
//Hit foward, placeholder value
robot.AASB.setPosition(0.7);
robot.AASB.setPosition(0.4);
}
robot.AAST.setPosition(0);
robot.colorSensor.enableLed(false);
robot.AAST.setPosition(0);
RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(template);
if(vuMark.equals(RelicRecoveryVuMark.CENTER)){
telemetry.addData("vuMark ", "In the center");
telemetry.update();
robot.driveDir(Direction.FORWARDS,36, Right, Left);
robot.turn(Direction.RIGHT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
//do some center stuff
}else if(vuMark.equals(RelicRecoveryVuMark.LEFT)){
telemetry.addData("vuMark ", "In the left");
telemetry.update();
robot.driveDir(Direction.BACKWARDS,25,Right,Left);
robot.driveDir(Direction.FORWARDS,28, Right, Left);
robot.turn(Direction.RIGHT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
//VUFORIA STUFF THAT IM NOT DOING RIGHT NOW
// do some left stuff
}else if(vuMark.equals(RelicRecoveryVuMark.RIGHT)){
telemetry.addData("vuMark ", "In the right");
telemetry.update();
robot.driveDir(Direction.FORWARDS,45, Right, Left);
robot.turn(Direction.RIGHT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
// do some right stuff
}else{
telemetry.addData("vuMark ", "who nose");
telemetry.update();
//Strafe towards the left;
robot.driveDir(Direction.FORWARDS,36, Right, Left);
robot.turn(Direction.RIGHT,90,Right,Left);
robot.driveDir(Direction.FORWARDS,10, Right, Left);
robot.turn(Direction.LEFT,90,Right,Left);
Side side = Side.CENTER;
// Side side is wahtever side its suppsdeo to be on
if(side.equals(Side.CENTER)){
//go put it in the center
}else if(side.equals(Side.LEFT)){
//go to teh left
}else if(side.equals(Side.RIGHT)){
//go to the right
}else if(side.equals(Side.NONEFOUND)){
//stop
//it's unknown, just go to the center i suppose
}
}
Oops, something went wrong.

0 comments on commit 083dbec

Please sign in to comment.