Skip to content

Commit

Permalink
added code to use new servo motors in teleop and autonomous
Browse files Browse the repository at this point in the history 
perfected baseOnlyRed and baseOnlyBlue
fixed straightLeft and straightRight autos
  • Loading branch information
@MayaExists
MayaExists committed Jan 22, 2020
1 parent de33618 commit bc04700
Show file tree
Hide file tree
Showing 5 changed files with 57 additions and 37 deletions.
26 changes: 9 additions & 17 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BaseMover.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,57 +26,50 @@ public BaseMover(Telemetry telemetry, HardwareInnov8Dobby robot, LinearOpMode op
this.robot = robot;
this.telemetry = telemetry;
this.telemetry.addData(BASE_MOVER_CAPTION,"Motors have been raised");
this.telemetry.addData("base motor current pos.", this.robot.baseMover.getCurrentPosition());
this.telemetry.update();
while (!this.robot.neville.isPressed() && this.robot.baseMover.getCurrentPosition() < endPosition) {
this.robot.baseMover.setPower(-0.5);
this.telemetry.addData("base motor current pos.", this.robot.baseMover.getCurrentPosition());
this.telemetry.update();
}
this.robot.baseMover.setPower(0);
this.robot.baseServoLeft.setPosition(startServo);
this.robot.bHSupport.setPosition((0.1));
this.robot.bHSupportRight.setPosition(0.75);
this.robot.baseServoRight.setPosition(0.1);
}

public void raiseMotors() {
this.telemetry.addData(BASE_MOVER_CAPTION,"Motors have been raised");
this.telemetry.update();
if (this.opMode.opModeIsActive()) {
this.robot.bHSupport.setPosition((0.1));
this.robot.bHSupportRight.setPosition(0.75);
try {
Thread.sleep(500);
}
catch(InterruptedException e){
Log.d("Spleepy time", "Sleep failed");
}
this.robot.baseServoLeft.setPosition(0.2);
while (!this.robot.neville.isPressed() && this.robot.baseMover.getCurrentPosition() < endPosition) {
this.robot.baseMover.setPower(-0.5);
}
this.robot.baseMover.setPower(0);
this.robot.baseServoRight.setPosition(0.1);

}

}

public void lowerMotors() {
if (this.opMode.opModeIsActive()) {
this.robot.baseServoLeft.setPosition(motorsDown);
this.robot.baseServoRight.setPosition(0.8);
try {
Thread.sleep(500);
}
catch(InterruptedException e){
Log.d("Spleepy time", "Sleep failed");
}
this.robot.bHSupport.setPosition(motorsDown);
while (this.opMode.opModeIsActive() && this.robot.baseMover.getCurrentPosition() > 0) {
this.robot.baseMover.setPower(0.5);
}
this.robot.baseMover.setPower(0);
this.robot.bHSupportRight.setPosition(0);
}

this.telemetry.addData(BASE_MOVER_CAPTION,"Motors have been lowered");
this.telemetry.update();
}


public void teleopUpdate(Gamepad gamepad1, Gamepad gamepad2){

this.telemetry.addData(BASE_MOVER_CAPTION, "gamepad updated");
Expand All @@ -89,7 +82,6 @@ public void teleopUpdate(Gamepad gamepad1, Gamepad gamepad2){
this.raiseMotors();
}
if (gamepad1.a/* && this.robot.baseMover.getCurrentPosition() > -1*/) {
Log.d("BaseMotorWorm Status","" + this.robot.baseMover.getCurrentPosition());
this.lowerMotors();
}

Expand Down
36 changes: 32 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Dobby.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,13 +133,37 @@ public void autoSimple() {
driveTrain.goForward(12); // assuming robot is 18" long
}
public void straightRight(){
driveTrain.goForward(24);
driveTrain.turn(-95);
driveTrain.goForward(15);
try {
Thread.sleep(5000);
}
catch(InterruptedException e){
Log.d("Spleepy time", "Sleep failed");
}
driveTrain.turn(-68);
try {
Thread.sleep(5000);
}
catch(InterruptedException e){
Log.d("Spleepy time", "Sleep failed");
}
driveTrain.goForward((30));
}
public void straightLeft(){
driveTrain.goForward(24);
driveTrain.turn(95);
driveTrain.goForward(15);
try {
Thread.sleep(5000);
}
catch(InterruptedException e){
Log.d("Spleepy time", "Sleep failed");
}
driveTrain.turn(68);
try {
Thread.sleep(5000);
}
catch(InterruptedException e){
Log.d("Spleepy time", "Sleep failed");
}
driveTrain.goForward((30));
}

Expand All @@ -157,6 +181,8 @@ public void blueBaseOnly() {
driveTrain.turn(90);
driveTrain.goForward(12);
baseMover.raiseMotors();
driveTrain.goLeft(21);
driveTrain.goRight(2);
driveTrain.goBackward(30);
baseMover.lowerMotors();
}
Expand All @@ -175,6 +201,8 @@ public void redBaseOnly() {
driveTrain.turn(-90);
driveTrain.goForward(12);
baseMover.raiseMotors();
driveTrain.goRight(21);
driveTrain.goLeft(2);
driveTrain.goBackward(30);
baseMover.lowerMotors();
}
Expand Down
15 changes: 7 additions & 8 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveTrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ public void goBackward(double inches) {
* @param degreesToTurn Number of degrees to turn. If negative, turns right. If positive, turns left.
*/
public void turn(double degreesToTurn) {
double turnCorrector = 0.725;
double turnCorrector = 1;
degreesToTurn = degreesToTurn * turnCorrector;
this.robot.imu.initialize(parameters);
angles = this.robot.imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
Expand All @@ -161,7 +161,7 @@ public void turn(double degreesToTurn) {
telemetry.addData("angles", angles.firstAngle);
telemetry.addData("degreesToTurn", degreesToTurn);
telemetry.update();
Log.d("Turning", "angles: " + angles.firstAngle + ", " + angles.secondAngle + ", " + angles.thirdAngle);
Log.d("Turning", "angles 164: " + angles.firstAngle + ", " + angles.secondAngle + ", " + angles.thirdAngle);
}
} else {
while ((angles.firstAngle < degreesToTurn) && this.opMode.opModeIsActive()) {
Expand All @@ -173,7 +173,7 @@ public void turn(double degreesToTurn) {
telemetry.addData("angles", angles.firstAngle);
telemetry.addData("degreesToTurn", degreesToTurn);
telemetry.update();
Log.d("Turning", "angles: " + angles.firstAngle + ", " + angles.secondAngle + ", " + angles.thirdAngle);
Log.d("Turning", "angles 176: " + angles.firstAngle + ", " + angles.secondAngle + ", " + angles.thirdAngle);
}
}
this.stop();
Expand All @@ -184,11 +184,10 @@ public void goLeft(double inches) {

startPosition = this.robot.motorOne.getCurrentPosition();
endPosition = startPosition - (inches * sideInchToTick); // How far you need to travel
while (this.robot.motorOne.getCurrentPosition() < endPosition && this.opMode.opModeIsActive()) {
Log.d("Dobby", "End Position: " + endPosition);
Log.d("Dobby", "Start Pos: " + startPosition);
Log.d("Dobby", "Current Pos: " + this.robot.motorOne.getCurrentPosition());

while (this.robot.motorOne.getCurrentPosition() > endPosition && this.opMode.opModeIsActive()) {
Log.d("going Left", "End Position: " + endPosition);
Log.d("going Left", "Start Pos: " + startPosition);
Log.d("going Left", "Current Pos: " + this.robot.motorOne.getCurrentPosition());

this.robot.motorOne.setPower(-wheelOnePower);
this.robot.motorTwo.setPower(wheelTwoPower);
Expand Down
15 changes: 8 additions & 7 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwareInnov8Dobby.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,15 @@ public class HardwareInnov8Dobby {
public DcMotor liftMotor = null; // Lift
public DcMotor liftMotor2 = null; //other Lift motor
public DcMotor handMotor = null;
public DcMotor baseMover = null;


// Example for servos

// public Servo handServo = null;
public Servo baseServoLeft = null;
public Servo baseServoLeft = null; // actually the right
public Servo bHSupport = null;
public Servo baseServoRight = null;
public Servo bHSupportRight = null;
public Servo ginny = null;
public CRServo rapServoLeft = null;
public CRServo rapServoRight = null;
Expand Down Expand Up @@ -84,15 +85,13 @@ public void init(HardwareMap ahwMap) {
liftMotor = this.hwMap.dcMotor.get("liftMotor");
liftMotor2 = this.hwMap.dcMotor.get("liftMotor2");
handMotor = this.hwMap.dcMotor.get("handMotor");
baseMover = this.hwMap.dcMotor.get("baseMover");
motorOne.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
motorTwo.setDirection(DcMotor.Direction.REVERSE);// Set to FORWARD if using AndyMark motors
motorThree.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
motorFour.setDirection(DcMotor.Direction.REVERSE);// Set to FORWARD if using AndyMark motors
liftMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
liftMotor2.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
handMotor.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
baseMover.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors

// //leftSensor = this.hwMap.colorSensor.get("leftSensor");
dumbledore = this.hwMap.colorSensor.get("dumbledore");
Expand All @@ -113,7 +112,6 @@ public void init(HardwareMap ahwMap) {
liftMotor.setPower(0);
liftMotor2.setPower(0);
handMotor.setPower(0);
baseMover.setPower(0);


// Set all motors to run without encoders.
Expand All @@ -126,7 +124,6 @@ public void init(HardwareMap ahwMap) {
liftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
liftMotor2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
handMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
baseMover.setMode(DcMotor.RunMode.RUN_USING_ENCODER);


// Define and initialize ALL installed servos.
Expand All @@ -135,9 +132,13 @@ public void init(HardwareMap ahwMap) {
//handServo = hwMap.servo.get("handServo");
//handServo.setPosition(END_SERVO);
baseServoLeft = hwMap.servo.get("baseServoLeft");
baseServoLeft.setPosition(0.2);
baseServoLeft.setPosition(0);
bHSupport = hwMap.servo.get("bHSupport");
bHSupport.setPosition(0.1);
baseServoRight = hwMap.servo.get("baseServoRight");
baseServoRight.setPosition(0.1);
bHSupportRight = hwMap.servo.get("bHSupportRight");
bHSupportRight.setPosition(0.75);
ginny = hwMap.servo.get("ginny");
ginny.setPosition(0.6);
rapServoLeft = hwMap.crservo.get("rapServoLeft");
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StraightLeft_Dobby.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ public class StraightLeft_Dobby extends LinearOpMode
public void runOpMode() throws InterruptedException {
Dobby dobby = new Dobby(telemetry, hardwareMap, this);
waitForStart();
dobby.straightRight();
dobby.straightLeft();

}
}

0 comments on commit bc04700

Please sign in to comment.