Skip to content

Commit

Permalink
Integrated angle encoder values and autonomous red with encoder values.
Browse files Browse the repository at this point in the history
  • Loading branch information
@OviedoRobotics
OviedoRobotics committed Dec 3, 2024
1 parent 31068f4 commit a6540b6
Show file tree
Hide file tree
Showing 7 changed files with 56 additions and 38 deletions.
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -367,7 +367,7 @@ else if( autoViperMotorTimer.milliseconds() > 3000 ) {
void autoTiltMotorMoveToTarget(double targetArmAngle )
{
// Convert angle to encoder counts
int targetEncoderCount = robot.computeEncoderCountsFromAngle(robot.armTiltAngle);
int targetEncoderCount = Hardware2025Bot.computeEncoderCountsFromAngle(targetArmAngle);
// Configure target encoder count
robot.wormTiltMotor.setTargetPosition( targetEncoderCount );
// Enable RUN_TO_POSITION mode
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,7 +312,7 @@ private void level1Ascent() {

if( opModeIsActive() ) {
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_DEG);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ASCENT1_DEG);
timeDriveStraight(-DRIVE_SPEED_20,3000);
do {
if( !opModeIsActive() ) break;
Expand Down
2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,7 +312,7 @@ private void level1Ascent() {

if( opModeIsActive() ) {
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_DEG);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ASCENT1_DEG);
timeDriveStraight(-DRIVE_SPEED_20,3000);
do {
if( !opModeIsActive() ) break;
Expand Down
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightBlue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -336,11 +336,11 @@ private void herdSamples(int samplesToHerd) {
pos_angle=-5.0; // angle the bot away from the wall as we herd the final sample
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_60, TURN_SPEED_40, DRIVE_THRU );
// Go fast to the edge of the observation zone
pos_y = 14.0;
pos_y = 15.0;
pos_x -= 3.0; // end 5" away from the wall
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_80, TURN_SPEED_40, DRIVE_THRU );
// ease into the observation zone (in case we hit the wall, or another robot)
timeDriveStraight(-DRIVE_SPEED_20,1500);
timeDriveStraight(-DRIVE_SPEED_20,1000);
} // opModeIsActive
// If we did any herding, turn off the motors
if (samplesToHerd > 0) {
Expand Down
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousRightRed.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -336,11 +336,11 @@ private void herdSamples(int samplesToHerd) {
pos_angle=-5.0; // angle the bot away from the wall as we herd the final sample
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_60, TURN_SPEED_40, DRIVE_THRU );
// Go fast to the edge of the observation zone
pos_y = 14.0;
pos_y = 15.0;
pos_x -= 3.0; // end 5" away from the wall
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_80, TURN_SPEED_40, DRIVE_THRU );
// ease into the observation zone (in case we hit the wall, or another robot)
timeDriveStraight(-DRIVE_SPEED_20,1500);
timeDriveStraight(-DRIVE_SPEED_20,1000);
} // opModeIsActive
// If we did any herding, turn off the motors
if (samplesToHerd > 0) {
Expand Down
70 changes: 44 additions & 26 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardware2025Bot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,32 +101,37 @@ public class Hardware2025Bot

protected AnalogInput armTiltEncoder = null; // US Digital absolute magnetic encoder (MA3)
public double armTiltAngle = 0.0; // 0V = 0 degrees; 3.3V = 359.99 degrees
public double armTiltAngleOffset = 129.0; // allows us to adjust the 0-360 deg range
public double turretAngleTarget = 0.0; // Automatic movement target angle (degrees)
public static final double armTiltAngleOffset = 129.0; // allows us to adjust the 0-360 deg range
public double turretAngleTarget = 0.0; // Automatic movement target angle (degrees)

public int TILT_ANGLE_HW_MAX = 3675; // encoder at maximum rotation UP/BACK (horizontal = -200)
public int TILT_ANGLE_BASKET = 3675; // encoder at rotation back to the basket for scoring
public int TILT_ANGLE_RAISED = 2000; // encoder at rotation back to the basket for scoring
public int TILT_ANGLE_HANG1 = 1400; // encoder when preparing for level 2 ascent
public int TILT_ANGLE_HANG2 = 400; // encoder at the end of level 2 ascent
public int TILT_ANGLE_ZERO = 0; // encoder for parking fullyh reset in auto
public int TILT_ANGLE_DRIVE = 200; // encoder for parking in auto or driving around
public int TILT_ANGLE_AUTO1 = 2005; // tilted up for autonomous specimen scoring (above bar)
public int TILT_ANGLE_AUTO2 = 1780; // tilted up for autonomous specimen scoring (clipped)
public int TILT_ANGLE_HW_MIN = -2000; // encoder at maximum rotation DOWN/FWD

public static double startingTurretAngle = 97.0;
public final static double ENCODER_COUNTS_PER_DEG = 5675.0 / 90.9;
public final static double TILT_ANGLE_HW_MAX_DEG = 38.1; // encoder at maximum rotation UP/BACK (horizontal = -200)
public final static double TILT_ANGLE_BASKET_DEG = 38.1; // encoder at rotation back to the basket for scoring
public final static double TILT_ANGLE_RAISED_DEG = 64.9; // encoder at rotation back to the basket for scoring
public final static double TILT_ANGLE_HANG1_DEG = 74.5; // encoder when preparing for level 2 ascent
public final static double TILT_ANGLE_HANG2_DEG = 90.6; // encoder at the end of level 2 ascent
public final static double TILT_ANGLE_ZERO_DEG = 97.0; // encoder for parking fullyh reset in auto
public final static double TILT_ANGLE_DRIVE_DEG = 93.8; // encoder for parking in auto or driving around
public final static double TILT_ANGLE_AUTO1_DEG = 64.9; // tilted up for autonomous specimen scoring (above bar)
public final static double TILT_ANGLE_AUTO2_DEG = 68.5; // tilted up for autonomous specimen scoring (clipped)
public final static double TILT_ANGLE_HW_MIN_DEG = 129.0; // encoder at maximum rotation DOWN/FWD
public int TILT_ANGLE_BASKET = 3675; // 93.8 deg at 3582 encoder at rotation back to the basket for scoring
public int TILT_ANGLE_RAISED = 2000; // 54.5 deg encoder at rotation back to the basket for scoring
public int TILT_ANGLE_HANG1 = 1400; // 40.1 deg encoder when preparing for level 2 ascent
public int TILT_ANGLE_HANG2 = 400; // 16.4 deg encoder at the end of level 2 ascent
public int TILT_ANGLE_ZERO = 0; // 7 deg encoder for parking fullyh reset in auto
public int TILT_ANGLE_DRIVE = 200; // 11.8 deg encoder for parking in auto or driving around
public int TILT_ANGLE_AUTO1 = 2005; // 54.8 deg tilted up for autonomous specimen scoring (above bar)
public int TILT_ANGLE_AUTO2 = 1780; // 49.6 tilted up for autonomous specimen scoring (clipped)
public int TILT_ANGLE_HW_MIN = -2000; // does not exist encoder at maximum rotation DOWN/FWD

// This value is set at init.
public static double startingArmTiltAngle = 0.0;
// Delta math from -0.1 deg -3891 encoder counts
// 94.4 deg 5 encoder counts
// 94.5 deg 3896 encoder counts range
public final static double ENCODER_COUNTS_PER_DEG = 3896.0 / 94.5;
public final static double TILT_ANGLE_HW_MAX_DEG = 95.00; // encoder at maximum rotation UP/BACK (horizontal = -200)
public final static double TILT_ANGLE_BASKET_DEG = 95.00; // encoder at rotation back to the basket for scoring
public final static double TILT_ANGLE_ASCENT1_DEG = 93.80; // encoder at rotation back to the low bar for ascent level 1
public final static double TILT_ANGLE_RAISED_DEG = 54.50; // encoder at rotation back to the basket for scoring
public final static double TILT_ANGLE_HANG1_DEG = 40.10; // encoder when preparing for level 2 ascent
public final static double TILT_ANGLE_HANG2_DEG = 16.40; // encoder at the end of level 2 ascent
public final static double TILT_ANGLE_ZERO_DEG = 7.00; // encoder for parking fullyh reset in auto
public final static double TILT_ANGLE_DRIVE_DEG = 11.80; // encoder for parking in auto or driving around
public final static double TILT_ANGLE_AUTO1_DEG = 54.80; // tilted up for autonomous specimen scoring (above bar)
public final static double TILT_ANGLE_AUTO2_DEG = 49.60; // tilted up for autonomous specimen scoring (clipped)
public final static double TILT_ANGLE_HW_MIN_DEG = 0.00; // encoder at maximum rotation DOWN/FWD

//====== Viper slide MOTOR (RUN_USING_ENCODER) =====
protected DcMotorEx viperMotor = null;
Expand Down Expand Up @@ -269,7 +274,7 @@ public void init(HardwareMap ahwMap, boolean isAutonomous ) {
wormPanMotor = hwMap.get(DcMotorEx.class,"WormPan"); // Control Hub port 0
wormTiltMotor = hwMap.get(DcMotorEx.class,"WormTilt"); // Control Hub port 1
armTiltEncoder = hwMap.get(AnalogInput.class, "tiltMA3"); // Expansion Hub analog 0
startingTurretAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset);
startingArmTiltAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset);

wormPanMotor.setDirection(DcMotor.Direction.FORWARD);
wormTiltMotor.setDirection(DcMotor.Direction.FORWARD);
Expand Down Expand Up @@ -394,6 +399,19 @@ public double computeAbsoluteAngle( double measuredVoltage, double zeroAngleOffs
return correctedAngle;
} // computeAbsoluteAngle

public double computeRawAngle( double measuredVoltage )
{
final double DEGREES_PER_ROTATION = 360.0; // One full rotation measures 360 degrees
final double MAX_MA3_ANALOG_VOLTAGE = 3.3; // 3.3V maximum analog output
// NOTE: when vertical the angle is 38.1deg, when horizontal 129.0 (prior to offset below)
double measuredAngle = (measuredVoltage / MAX_MA3_ANALOG_VOLTAGE) * DEGREES_PER_ROTATION;
// Correct for the offset angle (see note above)
// Enforce that any wrap-around remains in the range of -180 to +180 degrees
while( measuredAngle < -180.0 ) measuredAngle += 360.0;
while( measuredAngle > +180.0 ) measuredAngle -= 360.0;
return measuredAngle;
} // computeAbsoluteAngle

/*--------------------------------------------------------------------------------------------*/
/* NOTE: The absolute magnetic encoders may not be installed with 0deg rotated to the "right" */
/* rotational angle to put ZERO DEGREES where we want it. By defining a starting offset, and */
Expand All @@ -402,7 +420,7 @@ public double computeAbsoluteAngle( double measuredVoltage, double zeroAngleOffs
/*--------------------------------------------------------------------------------------------*/
public static int computeEncoderCountsFromAngle( double angle )
{
return (int)((startingTurretAngle - angle) * ENCODER_COUNTS_PER_DEG);
return (int)((angle - startingArmTiltAngle) * ENCODER_COUNTS_PER_DEG);
} // computeEncoderCountsFromAngle

/*--------------------------------------------------------------------------------------------*/
Expand Down
10 changes: 5 additions & 5 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -238,7 +238,7 @@ public void runOpMode() throws InterruptedException {
// telemetry.addData("Front", "%d %d counts", robot.frontLeftMotorPos, robot.frontRightMotorPos );
// telemetry.addData("Back ", "%d %d counts", robot.rearLeftMotorPos, robot.rearRightMotorPos );
telemetry.addData("Pan", "%d counts", robot.wormPanMotorPos );
telemetry.addData("Tilt", "%d counts %.1f deg", robot.wormTiltMotorPos, robot.armTiltAngle);
telemetry.addData("Tilt", "%d counts %.1f deg %.1f raw deg", robot.wormTiltMotorPos, robot.armTiltAngle, robot.computeRawAngle(robot.armTiltEncoder.getVoltage()));
telemetry.addData("Viper", "%d counts", robot.viperMotorPos );
telemetry.addData("Elbow", "%.1f (%.1f deg)", robot.getElbowServoPos(), robot.getElbowServoAngle() );
telemetry.addData("Wrist", "%.1f (%.1f deg)", robot.getWristServoPos(), robot.getElbowServoAngle() );
Expand Down Expand Up @@ -593,8 +593,8 @@ else if(panAngleTweaked) {
void processTiltControls() {
// The encoder is backwards from our definition of MAX and MIN. Maybe change the
// convention in hardware class?
boolean safeToManuallyLower = (robot.armTiltAngle < robot.TILT_ANGLE_HW_MIN_DEG);
boolean safeToManuallyRaise = (robot.armTiltAngle > robot.TILT_ANGLE_HW_MAX_DEG);
boolean safeToManuallyLower = (robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_HW_MIN_DEG);
boolean safeToManuallyRaise = (robot.armTiltAngle < Hardware2025Bot.TILT_ANGLE_HW_MAX_DEG);
double gamepad2_right_stick = gamepad2.right_stick_y;
boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.08 );

Expand Down Expand Up @@ -664,7 +664,7 @@ else if( gamepad2_dpad_right_now && !gamepad2_dpad_right_last)
// Check for an OFF-to-ON toggle of the gamepad2 DPAD DOWN
else if( gamepad2_dpad_down_now && !gamepad2_dpad_down_last)
{ // Retract lift to the collection position
boolean armRaised = (robot.armTiltAngle < Hardware2025Bot.TILT_ANGLE_RAISED_DEG)? true: false;
boolean armRaised = (robot.armTiltAngle > Hardware2025Bot.TILT_ANGLE_RAISED_DEG)? true: false;
// If raised to the basket, do this automatically before lowering
// (don't want to do it if we use this backing out of submersimble
if (armRaised) {
Expand Down Expand Up @@ -824,7 +824,7 @@ void processLevel2Ascent() {
robot.viperMotor.setPower( -0.10 ); // hold power
}
// Do we need to further store the robot drivetrain?
boolean tiltRetractionIncomplete = (robot.armTiltAngle > robot.TILT_ANGLE_HANG2_DEG)? true : false;
boolean tiltRetractionIncomplete = (robot.armTiltAngle < Hardware2025Bot.TILT_ANGLE_HANG2_DEG)? true : false;
boolean tiltMotorLoadOkay = (robot.wormTiltMotorAmps < 8.5)? true : false;
if( viperRetractionIncomplete && viperMotorLoadOkay ) {
robot.wormTiltMotor.setPower( 0.30 ); // test at lower power!!!
Expand Down

0 comments on commit a6540b6

Please sign in to comment.