Skip to content

Commit

Permalink
Merge branch 'master' into intake_slides_code
Browse files Browse the repository at this point in the history
  • Loading branch information
@ShreyasT-1
ShreyasT-1 authored Nov 9, 2024
2 parents fa521a6 + 25c4b99 commit fb4cf8f
Show file tree
Hide file tree
Showing 11 changed files with 148 additions and 102 deletions.
68 changes: 0 additions & 68 deletions ...e/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java
View file

This file was deleted.

105 changes: 105 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoreArm.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;

import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.TouchSensor;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestSwitch;
import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoMotors;
import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController;

import java.util.Arrays;
import java.util.List;

public class ScoreArm extends QQMechanism{
public static final double TEST_SPEED = 0.55;
public static double TEST_SPEED_OFF = 0.4;
DcMotor leftMotor;
DcMotor rightMotor;
TouchSensor limitSwitch;
public static double kP = 0.01;
public static double kI = 0.0;
public static double kD = 0.0;
public static double kF = 0;
public static double max = 0.8;
public static double min = -0.8;

public int currentPos;
public int desiredPos;
public double motorPower;

static int INTAKE_POSITION = 0;
static int SCORING_POSITION = 350;
static int PLACING_POSITION = 750;

public Telemetry telemetry;


PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min);

@Override
public void init(HardwareMap hwMap) {
leftMotor = hwMap.get(DcMotor.class, "left_score_arm_motor");
rightMotor = hwMap.get(DcMotor.class, "right_score_arm_motor");
limitSwitch = hwMap.get(TouchSensor.class, "score_arm_switch");
rightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
leftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}

public void goToIntake(){
desiredPos = INTAKE_POSITION;
}
public void goToScoring(){
desiredPos = SCORING_POSITION;
}
public void goToPlace(){
desiredPos = PLACING_POSITION;
}
public void manualPositionChange(double changeAmount){
desiredPos += changeAmount;
}




@Override
public void update(){
if(limitSwitch.isPressed()) {
leftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
leftMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
rightMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
rightMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
if (desiredPos < 0){
desiredPos = 0;
}
}
currentPos = (leftMotor.getCurrentPosition() + rightMotor.getCurrentPosition())/2;//average left and right speeds
double motorPower = pidfController.calculate(desiredPos,currentPos);
this.motorPower = motorPower;
leftMotor.setPower(motorPower);
rightMotor.setPower(motorPower);
telemetry.addData("curerent pos",currentPos);
telemetry.addData("desired pos",desiredPos);
telemetry.addData("motor power",motorPower);

}

@Override
public List<QQTest> getTests() {
return Arrays.asList(
new TestTwoMotors("score arm up", TEST_SPEED,leftMotor,rightMotor),
new TestTwoMotors("score arm down", -TEST_SPEED, leftMotor,rightMotor),
new TestSwitch("Arm limit switch", limitSwitch)
);
}
}
3 changes: 2 additions & 1 deletion ...Code/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueCloseParkAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,8 @@ public void loop(){
step = 2;
}if(step == 2){
telemetry.addData("parked", 0);
robot.arm.goToGround();
robot.scoreArm.goToIntake();

}
}
}
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/BlueFarParkAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ public void loop(){
step = 2;
}if(step == 2){
telemetry.addData("parked", 0);
robot.arm.goToGround();
robot.scoreArm.goToIntake();
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/CloseParkAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ else if(step == 1){
if(doneDriving){
step = 3;
}if(step == 3){
robot.arm.goToGround();
robot.scoreArm.goToIntake();
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/FarParkAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ else if(step == 1){
if(doneDriving){
step = 3;
}if(step == 3){
robot.arm.goToGround();
robot.scoreArm.goToIntake();
}
}
}
Expand Down
26 changes: 14 additions & 12 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,10 @@ public class NoTurnTeleop extends QQOpMode{
public void init(){
isPlacing = false;
super.init();
intake_slides_code
robot.arm.telemetry = telemetry;
robot.intakeSlides.telemetry = telemetry;
robot.scoreArm.telemetry = telemetry;
}
public void loop(){
super.loop();
Expand All @@ -30,18 +32,26 @@ public void loop(){
} else robot.mecanumDrive.setSpeed(MecanumDrive.Speed.NORMAL);

if (gamepad1.a){
robot.arm.goToIntake();
robot.scoreArm.goToIntake();
robot.claw.open();
}else if (gamepad1.b) {
robot.claw.close();
robot.arm.goToUnderPlacement();
robot.scoreArm.goToPlace();
}else if (gamepad1.dpad_right){
robot.claw.wristStart();
}else if (gamepad1.dpad_left) {
robot.claw.wristEnd();
}else if (gamepad1.dpad_up){
intake_slides_code
robot.arm.manualPositionChange(MANUAL_CHANGE);
}else if (gamepad1.dpad_down){{

robot.scoreArm.manualPositionChange(MANUAL_CHANGE);
}else if (gamepad1.dpad_down){
robot.scoreArm.manualPositionChange(-MANUAL_CHANGE);
}
if (gamepad1.y && gamepad1.dpad_right){

robot.controlHub.resetGyro();
}
robot.arm.manualPositionChange(-MANUAL_CHANGE);
Expand All @@ -54,19 +64,11 @@ public void loop(){


if(gamepad1.x){
robot.arm.placeUnder();
robot.scoreArm.goToScoring();
isPlacing = true;
}else if(!gamepad1.x && isPlacing){
robot.claw.open();
robot.arm.goToIntake();








robot.scoreArm.goToIntake();

isPlacing = false;
}else{
Expand Down
1 change: 0 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/QQOpMode.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@ public void init() {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
robot.controlHub.resetGyro();
robot.claw.close();
robot.arm.goToDrive();
}

@Override
Expand Down
18 changes: 9 additions & 9 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ public class ScrimmageTeleop extends QQOpMode{
public void init(){
isPlacing = false;
super.init();
robot.arm.telemetry = telemetry;
robot.scoreArm.telemetry = telemetry;
}
public void loop(){
super.loop();
Expand All @@ -28,19 +28,19 @@ public void loop(){
} else robot.mecanumDrive.setSpeed(MecanumDrive.Speed.NORMAL);

if (gamepad1.a){
robot.arm.goToIntake();
robot.scoreArm.goToIntake();
robot.claw.open();
}else if (gamepad1.b) {
robot.claw.close();
robot.arm.goToPlacement();
robot.scoreArm.goToPlace();
}else if (gamepad1.dpad_right){
robot.claw.wristStart();
}else if (gamepad1.dpad_left) {
robot.claw.wristEnd();
}else if (gamepad1.dpad_up){
robot.arm.manualPositionChange(MANUAL_CHANGE);
robot.scoreArm.manualPositionChange(MANUAL_CHANGE);
}else if (gamepad1.dpad_down){
robot.arm.manualPositionChange(-MANUAL_CHANGE);
robot.scoreArm.manualPositionChange(-MANUAL_CHANGE);
}
if (gamepad1.y && gamepad1.dpad_right){
robot.controlHub.resetGyro();
Expand All @@ -49,13 +49,13 @@ public void loop(){
robot.claw.close();
}else if (gamepad1.right_bumper){
robot.claw.open();}

if (robot.arm.isAboveWristThreshold()){
/*
if (robot.scoreArm.isAboveWristThreshold()){
robot.claw.wristEnd();
}else {robot.claw.wristStart();}
}else {robot.claw.wristStart();}*/

if(gamepad1.x){
robot.arm.place();
robot.scoreArm.goToPlace();
robot.mecanumDrive.move(-.6,0,0);
isPlacing = true;
}else if(!gamepad1.x && isPlacing){
Expand Down
16 changes: 11 additions & 5 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,13 @@

import com.qualcomm.robotcore.hardware.HardwareMap;

import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm;
import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw;
import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub;
import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.IntakeSlides;
import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.OpticalTrackingOdometrySensor;
import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.QQMechanism;
import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ScoreArm;

import java.util.Arrays;
import java.util.List;
Expand All @@ -21,8 +21,12 @@ public class Robot {
public Claw claw;
//public DoubleReverse4Bar doubleReverse4Bar;
// public Slides slides;
public Arm arm;


public IntakeSlides intakeSlides;

public ScoreArm scoreArm;

List<QQMechanism> mechanisms;

public Robot() {
Expand All @@ -33,19 +37,21 @@ public Robot() {
intakeSlides = new IntakeSlides();
//doubleReverse4Bar = new DoubleReverse4Bar();
//slides = new Slides();
arm = new Arm();
scoreArm = new ScoreArm();

mechanisms = Arrays.asList(
controlHub,
mecanumDrive,
otos,
// otos,
claw,
// slides,
claw,

//doubleReverse4Bar,
intakeSlides,
arm
);
scoreArm);

}
public void init(HardwareMap hwMap) {
for (QQMechanism mechanism : mechanisms) {
Expand Down
Loading

0 comments on commit fb4cf8f

Please sign in to comment.