Skip to content

Commit

Permalink
Merge pull request #29 from ftc16072/TunePotAndImport
Browse files Browse the repository at this point in the history 
Code run at scrimmage
  • Loading branch information
@alan412
alan412 authored Nov 21, 2020
2 parents cff2b54 + baf0d7f commit fbf0175
Show file tree
Hide file tree
Showing 9 changed files with 121 additions and 36 deletions.
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/ShootAction.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ public QQ_Action run(QQ_Opmode opmode) {
switch (step){
case 0:
opmode.robot.shooter.spinWheels(0.8, 0.5);
opmode.robot.shooter.setPivotAngle(AUTO_SHOOT_ANGLE);
//opmode.robot.shooter.setPivotAngle(AUTO_SHOOT_ANGLE);
waitTime = opmode.time + 0.5;
step = 1;
break;
Expand Down
32 changes: 24 additions & 8 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/actions/TeleopDriveAction.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
import org.firstinspires.ftc.teamcode.utils.RobotPose;

public class TeleopDriveAction extends QQ_Action {
double pivotAngle;
double pivotAngle = 0;
boolean wasUp;
boolean wasDown;
final static double MAX_SPEED = 1;
Expand All @@ -35,7 +35,7 @@ public QQ_Action run(QQ_Opmode opmode) {
void manipulatorControls(QQ_Opmode opmode) {
//spinning shooter wheels
if (opmode.qq_gamepad2.rightBumper()) {
opmode.robot.shooter.spinWheels(0.8, 0.5);
opmode.robot.shooter.spinWheels(-1, -0.8);
} else {
opmode.robot.shooter.spinWheels(0, 0);
}
Expand All @@ -47,18 +47,19 @@ void manipulatorControls(QQ_Opmode opmode) {
} else {
opmode.robot.shooter.flick(false);
}

/*
//up and down of pivot shooter
if (opmode.qq_gamepad2.dpadUp() && !wasUp) {
pivotAngle = Math.max(1.0, pivotAngle + 0.1);
pivotAngle = Math.min(15, pivotAngle + 3);
}
wasUp = opmode.qq_gamepad2.dpadUp();
if (opmode.qq_gamepad2.dpadDown() && !wasDown) {
pivotAngle = Math.min(0, pivotAngle - 0.1);
pivotAngle = Math.max(0, pivotAngle - 3);
}
wasDown = opmode.qq_gamepad2.dpadDown();
opmode.robot.shooter.setPivotAngle(pivotAngle);
opmode.telemetry.addData("angle", pivotAngle);
opmode.robot.shooter.goToAngle(pivotAngle);
//up and down of wobbly goal
if (opmode.qq_gamepad2.leftStick.getY() > 0.2) {
Expand All @@ -73,12 +74,12 @@ void manipulatorControls(QQ_Opmode opmode) {
} else {
opmode.robot.wobblyGoal.closeGrabber();
}

*/
//intake and transfer in and out
if (opmode.qq_gamepad2.rightStick.getY() > 0.2) {
opmode.robot.intake.changeState(Intake.intakeState.Start);
opmode.robot.transfer.changeTransfer(Transfer.transferState.Start);
} else if (opmode.qq_gamepad2.rightStick.getY() < 0.2) {
} else if (opmode.qq_gamepad2.rightStick.getY() < -0.2) {
opmode.robot.intake.changeState(Intake.intakeState.Reverse);
opmode.robot.transfer.changeTransfer(Transfer.transferState.Reverse);
} else {
Expand All @@ -95,6 +96,21 @@ void manipulatorControls(QQ_Opmode opmode) {
public void driverControls(QQ_Opmode opmode) {
opmode.qq_gamepad1.leftStick.setSquared(true);




if (opmode.qq_gamepad1.dpadUp()){
opmode.robot.nav.resetIMU(0, AngleUnit.DEGREES);
} else if (opmode.qq_gamepad1.dpadLeft()){
opmode.robot.nav.resetIMU(-90, AngleUnit.DEGREES);
} else if (opmode.qq_gamepad1.dpadDown()){
opmode.robot.nav.resetIMU(180, AngleUnit.DEGREES);
} else if (opmode.qq_gamepad1.dpadRight()){
opmode.robot.nav.resetIMU(90, AngleUnit.DEGREES);
}



if (opmode.qq_gamepad1.leftBumper()) {
opmode.robot.mecanumDrive.setMaxSpeed(SLOW_SPEED);
} else if (opmode.qq_gamepad1.rightBumper()) {
Expand Down
6 changes: 3 additions & 3 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ public enum intakeState{
};

private DcMotor intakeMotor;
private static final double intakeSpeed = 0.2;
private static final double reverseIntakeSpeed = -0.2;
private static final double intakeSpeed = -0.5;
private static final double reverseIntakeSpeed = 0.2;

/**
* initialize intake
Expand All @@ -35,7 +35,7 @@ public void init(HardwareMap hwMap) {
*/
@Override
public List<QQ_Test> getTests() {
return Arrays.asList(new QQ_TestMotor("Intake Motor", 0.25, intakeMotor));
return Arrays.asList(new QQ_TestMotor("Intake Motor", -0.5, intakeMotor));
}

/**
Expand Down
22 changes: 14 additions & 8 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Navigation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,18 +17,17 @@

public class Navigation {
// Public Classe
public RobotPose currentPosition;
public BNO055IMU imu;
private double imuOffset;
public static RobotPose currentPosition;

// Private Classes
private static BNO055IMU imu;
MecanumDrive mecanumDrive;
double TRANSLATE_KP = 0.1;
double MIN_R = 0.14;



// Private Values
private double angleTolerance = AngleUnit.RADIANS.fromDegrees(2);
private static double imuOffset;
double TRANSLATE_KP = 0.1;
double MIN_R = 0.14;

/**
* Constructor
Expand Down Expand Up @@ -68,7 +67,14 @@ public void setCurrentPosition(RobotPose pose){
currentPosition = pose;
}

private double getHeading(AngleUnit au){
public void resetIMU(double angle, AngleUnit au) {
double supposedHeading = au.toRadians(angle);
double currentHeading = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS).firstAngle; //if we use our get method it would double the offset if we set it again

imuOffset = supposedHeading - currentHeading;
}

public double getHeading(AngleUnit au){
Orientation angles;

angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.RADIANS);
Expand Down
62 changes: 50 additions & 12 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Shooter.java
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,14 @@
package org.firstinspires.ftc.teamcode.mechanisms;

import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_DualTest;
import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_Test;
import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestAnalogSensor;
import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestMotor;
import org.firstinspires.ftc.teamcode.mechanisms.tests.QQ_TestServo;

Expand All @@ -22,16 +25,33 @@ enum AimLocation {
PowerShot3
}

public double shooterAngle = 0;


private DcMotor shooterBack;
private DcMotor shooterFront;
private Servo shooterPivot;
private Servo shooterImport;
private AnalogInput pot;

final double PIVOT_DOWN = 0.25; // TODO: find right value
final double PIVOT_UP = 0.75; // TODO: Find correct value
final double INSERT = 0.25; // TODO: Find Value
final double RESET = 0.75; // TODO: Find value

final double ANGLE_UP = 15.0;
final double ANGLE_DOWN = 0.0;

final double VOLT_UP = 0.17;
final double VOLT_DOWN = 0.27;

final double VOLTAGE_TOLERANCE = 0.03;
final double VOLTAGE_KP_UP = 1.6;
final double VOLTAGE_KP_DOWN = 0.8;




final double INSERT = 0; // TODO: Find Value
final double RESET = 0.18; // TODO: Find value

/**
* initializes Shooter
Expand All @@ -44,6 +64,8 @@ public void init(HardwareMap hwMap) {
shooterFront = hwMap.get(DcMotor.class, "shooter_front_motor");
shooterPivot = hwMap.get(Servo.class, "servo_pivot_shooter");
shooterImport = hwMap.get(Servo.class, "servo_import_shooter");
pot = hwMap.get(AnalogInput.class, "lift_pot");

}

/**
Expand All @@ -55,7 +77,9 @@ public void init(HardwareMap hwMap) {
public List<QQ_Test> getTests() {
return Arrays.asList(new QQ_TestMotor("Front Motor", 0.25, shooterFront),
new QQ_TestMotor("Back Motor", 0.25, shooterBack),
new QQ_TestServo("Pivot", PIVOT_UP, PIVOT_DOWN, shooterPivot),
new QQ_DualTest(
new QQ_TestServo("Pivot", PIVOT_UP, PIVOT_DOWN, shooterPivot),
new QQ_TestAnalogSensor("pot", pot)),
new QQ_TestServo("Import", INSERT, RESET, shooterImport));
}

Expand All @@ -71,6 +95,29 @@ public boolean aim(AimLocation aimLocation) {
return false;
}


public boolean goToAngle(double angle){
return goToVoltage(Range.scale(angle, VOLT_DOWN, VOLT_UP, ANGLE_DOWN, ANGLE_UP));
}

private boolean goToVoltage(double desiredVoltage){
double voltageDiff = pot.getVoltage() - desiredVoltage;
if (Math.abs(voltageDiff) <= VOLTAGE_TOLERANCE){
shooterPivot.setPosition(0.0);
return true;
}

if(voltageDiff > 0){
double speed = Math.max( voltageDiff * VOLTAGE_KP_UP, 0.75);
shooterPivot.setPosition(speed);
} else {
double speed = Math.max( voltageDiff * VOLTAGE_KP_DOWN, 0.25);
shooterPivot.setPosition(speed);
}
return false;
}


public void spinWheels(double frontSpeed, double backSpeed) {
shooterBack.setPower(frontSpeed);
shooterFront.setPower(backSpeed);
Expand All @@ -84,15 +131,6 @@ public void flick(boolean shouldFlick){
}
}

/**
* sets shooter pivot
* @param angle 0 is down, 1 is up
*/
public void setPivotAngle(double angle){

shooterPivot.setPosition(Range.scale(angle, 0, 1, PIVOT_DOWN, PIVOT_UP));

}


/**
Expand Down
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/Transfer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ public enum transferState {
*/

private DcMotor transferMotor;
private static final double transferSpeed = 0.25;
private static final double transferSpeed = 0.5;
private static final double reverseTransferSpeed = -0.25;

@Override
Expand All @@ -38,7 +38,7 @@ public void init(HardwareMap hwMap) {
*/
@Override
public List<QQ_Test> getTests() {
return Arrays.asList(new QQ_TestMotor("Transfer Motor", 0.25, transferMotor));
return Arrays.asList(new QQ_TestMotor("Transfer Motor", 0.5, transferMotor));
}

public void changeTransfer(Transfer.transferState desiredState) {
Expand Down
25 changes: 25 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_DualTest.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
package org.firstinspires.ftc.teamcode.mechanisms.tests;

import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.Telemetry;

public class QQ_DualTest extends QQ_Test {
QQ_Test qq_test_1;
QQ_Test qq_test_2;


public QQ_DualTest(QQ_Test qq_test_1, QQ_Test qq_test_2){
super(qq_test_1.getDescription() + qq_test_2.getDescription());
this.qq_test_1 = qq_test_1;
this.qq_test_2 = qq_test_2;

}

@Override
public void run(boolean on, Telemetry telemetry) {
qq_test_1.run(on, telemetry);
qq_test_2.run(on, telemetry);
}
}
2 changes: 1 addition & 1 deletion ...de/src/main/java/org/firstinspires/ftc/teamcode/mechanisms/tests/QQ_TestAnalogSensor.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ public class QQ_TestAnalogSensor extends QQ_Test {
* @param description a human readable description of the test
* @param analogInput an instance of a analog input sensor the test is using
*/
QQ_TestAnalogSensor(String description, AnalogInput analogInput) {
public QQ_TestAnalogSensor(String description, AnalogInput analogInput) {
super(description);
this.analogInput = analogInput;
}
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opModes/QQ_ParkAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ public class QQ_ParkAuto extends QQ_Opmode {
double START_X = 48;
double TOLERANCE = 0.5;
double START_Y = 9;
double PARK_Y = 80;
double PARK_Y = 80.0 / 2;
@Override
public void init() {
super.init();
Expand Down

0 comments on commit fbf0175

Please sign in to comment.