Skip to content

Commit

Permalink
Merge pull request #35 from ftc16072/diffy-arm
Browse files Browse the repository at this point in the history 
Diffy arm
  • Loading branch information
@Joshua-Smith-42
Joshua-Smith-42 authored Dec 28, 2024
2 parents 5102484 + cf332c4 commit 3663598
Show file tree
Hide file tree
Showing 7 changed files with 154 additions and 110 deletions.
37 changes: 27 additions & 10 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,14 +8,15 @@
import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoServos;

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

@Config
public class IntakeArm extends QQMechanism {
public static double ARM_DROP_POSITION = 0.7;
public static double ARM_INTAKE_POSITION = 0.0;
public static double SEARCHING_POSITION = 0.12;
public static double TRANSFER_POSITION = 0.6;
private static final double MAX_SERVO_POS = 1;
public static double MANUAL_CHANGE_AMOUNT = 0.01;
private final static double ARM_DROP_POSITION = 0.1;
private final static double ARM_INTAKE_POSITION = 0.62;
Servo leftArmServo;
Servo rightArmServo;

Expand All @@ -35,18 +36,34 @@ public void goToDropPos(){
public void goToIntake(){
goToPos(ARM_INTAKE_POSITION);
}
public void searching(){
goToPos(SEARCHING_POSITION);


public void rotateArmLeft(){
rotateArm(MANUAL_CHANGE_AMOUNT);
}
public void rotateArmRight(){
rotateArm(-MANUAL_CHANGE_AMOUNT);
}
public void transfer(){
goToPos(TRANSFER_POSITION);

private void rotateArm(double manualChangeAmount){
double leftPos = leftArmServo.getPosition();
double rightPos = rightArmServo.getPosition();

leftPos = leftPos - manualChangeAmount;
rightPos = rightPos + manualChangeAmount;

if(((leftPos <= MAX_SERVO_POS) && (leftPos >= 0)) &&
((rightPos <= MAX_SERVO_POS) && (rightPos >= 0))){
leftArmServo.setPosition(leftPos);
rightArmServo.setPosition(rightPos);
}
}



@Override
public List<QQTest> getTests() {
return Arrays.asList(
new TestTwoServos("arm_pos", ARM_DROP_POSITION, ARM_INTAKE_POSITION, leftArmServo, rightArmServo));
return Collections.singletonList(
new TestTwoServos("arm_pos", ARM_INTAKE_POSITION, ARM_DROP_POSITION, leftArmServo, rightArmServo));
}
}
64 changes: 48 additions & 16 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeClaw.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,61 +3,93 @@
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

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.TestTwoServos;

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

@Config
public class IntakeClaw extends QQMechanism {
public static final double MAX_SERVO_POS = 1;
public static double CLAW_CLOSE_POSITION = 1;
public static double CLAW_OPEN_POSITION = 0.8;
public static double WRIST_FLAT_POSITION = 0.5;
public static double WRIST_START_POSITION = 0.15;
public static double WRIST_END_POSITION = 0.85;
public static double WRIST_START_POSITION = 0.0;
public static double WRIST_TRANSFER_POSITION = 0.65;

ElapsedTime openTimer = new ElapsedTime();
ElapsedTime closedTimer = new ElapsedTime();
Servo clawServo;
Servo wristServo;
double wristServoPos;
Servo leftWristServo;
Servo rightWristServo;
double CLOSED_TIME = 0.5;
double OPEN_TIME = 0.5;
double leftWristServoPos;
double rightWristServoPos;


@Override
public void init(HardwareMap hwMap) {
clawServo = hwMap.get(Servo.class, "intake_claw_servo");
wristServo = hwMap.get(Servo.class, "intake_wrist_servo");
leftWristServo = hwMap.get(Servo.class, "left_intake_wrist");
rightWristServo = hwMap.get(Servo.class,"right_intake_wrist");
leftWristServo.setDirection(Servo.Direction.REVERSE);
}

@Override
public List<QQTest> getTests() {
return Arrays.asList(
new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo),
new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo)
new TestTwoServos("wrist_movement", WRIST_START_POSITION, WRIST_TRANSFER_POSITION, leftWristServo,rightWristServo)
);
}


public void wristTransfer(){
leftWristServoPos = WRIST_START_POSITION;
rightWristServoPos = WRIST_START_POSITION;
}
public void wristIntake(){
leftWristServoPos = WRIST_TRANSFER_POSITION;
rightWristServoPos = WRIST_TRANSFER_POSITION;
}
public void open() {
openTimer.reset();
clawServo.setPosition(CLAW_OPEN_POSITION);
}

public void close() {
closedTimer.reset();
clawServo.setPosition(CLAW_CLOSE_POSITION);
}

public void wristFlat(){
wristServoPos = WRIST_FLAT_POSITION;
}
public void adjustWrist(double manualChangeAmount){
wristServoPos += manualChangeAmount;
wristServoPos = Math.min(WRIST_END_POSITION,wristServoPos);
wristServoPos = Math.max(WRIST_START_POSITION,wristServoPos);
if (leftWristServoPos <= MAX_SERVO_POS && rightWristServoPos <= MAX_SERVO_POS){
leftWristServoPos += manualChangeAmount;
rightWristServoPos -= manualChangeAmount;
}
} public boolean isClawOpen() {
if ((clawServo.getPosition() == CLAW_OPEN_POSITION) &&
(openTimer.seconds()> OPEN_TIME))
return true;

return false;
}
public boolean isClawClosed() {
if ((clawServo.getPosition() == CLAW_CLOSE_POSITION) &&
(closedTimer.seconds()> CLOSED_TIME))
return true;

return false;
}


@Override
public void update(Telemetry telemetry){
wristServo.setPosition(wristServoPos);
rightWristServo.setPosition(rightWristServoPos);
leftWristServo.setPosition(leftWristServoPos);
}

}
Expand Down
10 changes: 9 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ public class IntakeSlides extends QQMechanism {
public static final int SLIDES_EXTENSION_BOUNDARY = FULL_EXTENSION_POSITION+10;
private static final int HALF_EXTENSION_POSITION = 740;
private static final int START_POSITION = 0;
public static int MANUAL_CHANGE_AMOUNT = 30;

private DcMotor rightIntakeSlideMotor;
private DcMotor leftIntakeSlideMotor;
Expand Down Expand Up @@ -68,7 +69,13 @@ public void init(HardwareMap hwMap) {
public void setPosition(int desiredPos){
this.desiredPos = desiredPos;
}
public void manualPositionChange(int changeAmount){
public void extend(){
manualPositionChange(MANUAL_CHANGE_AMOUNT);
}
public void retract(){
manualPositionChange(-MANUAL_CHANGE_AMOUNT);
}
private void manualPositionChange(int changeAmount){
desiredPos += changeAmount;
}
@Override
Expand Down Expand Up @@ -106,6 +113,7 @@ public void update(Telemetry telemetry){
public boolean isSwitchPressed(){
return limitSwitch.isPressed();
}
public boolean isSafeToRotate() { return currentPos > HALF_EXTENSION_POSITION;}



Expand Down
14 changes: 1 addition & 13 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/ScoringClaw.java
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevTouchSensor;
import com.qualcomm.robotcore.hardware.ColorRangeSensor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
Expand All @@ -25,21 +24,18 @@ public class ScoringClaw extends QQMechanism {
public static final double CLOSED_TIME = 0.25;
public static double CLAW_CLOSE_POSITION = 0.5;
public static double CLAW_OPEN_POSITION = 0;
public static double WRIST_START_POSITION = 0;
public static double WRIST_END_POSITION = 0.67;



ElapsedTime openTimer = new ElapsedTime();
ElapsedTime closedTimer = new ElapsedTime();
Servo clawServo;
Servo wristServo;
ColorRangeSensor colorSensor;
TouchSensor scoreSwitch;

@Override
public void init(HardwareMap hwMap) {
clawServo = hwMap.get(Servo.class, "claw_servo");
wristServo = hwMap.get(Servo.class, "wrist_servo");
colorSensor = hwMap.get(ColorRangeSensor.class, "scoreclaw_color");
scoreSwitch = hwMap.get(TouchSensor.class,"score_switch");

Expand All @@ -50,7 +46,6 @@ public void init(HardwareMap hwMap) {
public List<QQTest> getTests() {
return Arrays.asList(
new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo),
new TestServo("wrist_movement", WRIST_START_POSITION, WRIST_END_POSITION, wristServo),
new TestColorRangeSensor("scoreclaw_color", colorSensor),
new TestSwitch("score_switch", scoreSwitch)
);
Expand Down Expand Up @@ -80,13 +75,6 @@ public boolean isBlockGrabbable() {
return false;
}

public void wristStart() {
wristServo.setPosition(WRIST_START_POSITION);
}

public void wristEnd() {
wristServo.setPosition(WRIST_END_POSITION);
}

public void open() {
clawServo.setPosition(CLAW_OPEN_POSITION);
Expand Down
Loading

0 comments on commit 3663598

Please sign in to comment.