Skip to content

Commit

Permalink
Convert to use angle encoder isntead of motor encoder values.
Browse files Browse the repository at this point in the history
  • Loading branch information
@OviedoRobotics
OviedoRobotics committed Dec 2, 2024
1 parent 27c1aae commit 31068f4
Show file tree
Hide file tree
Showing 7 changed files with 40 additions and 36 deletions.
6 changes: 4 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousBase.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -364,8 +364,10 @@ else if( autoViperMotorTimer.milliseconds() > 3000 ) {
} // autoViperMotorWaitToComplete

/*---------------------------------------------------------------------------------*/
void autoTiltMotorMoveToTarget(int targetEncoderCount )
void autoTiltMotorMoveToTarget(double targetArmAngle )
{
// Convert angle to encoder counts
int targetEncoderCount = robot.computeEncoderCountsFromAngle(robot.armTiltAngle);
// Configure target encoder count
robot.wormTiltMotor.setTargetPosition( targetEncoderCount );
// Enable RUN_TO_POSITION mode
Expand Down Expand Up @@ -480,7 +482,7 @@ protected double getError(double targetAngle) {
*/
protected double getAngleError(double targetAngle) {
// calculate error in -179 to +180 range (
double robotError = targetAngle - robot.headingAngle;
double robotError = targetAngle - robot.imuHeadingAngle;
while (robotError > 180.0) robotError -= 360.0;
while (robotError <= -180.0) robotError += 360.0;
return robotError;
Expand Down
8 changes: 4 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftBlue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,7 +189,7 @@ private void scoreSpecimenPreload() {
// Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!)
pos_y=3.0; pos_x=0.0; pos_angle=0.0; // start at this absolute location
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO1);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG);
pos_y += 3.0;
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1);
Expand Down Expand Up @@ -228,7 +228,7 @@ private void scoreSpecimenPreload() {
// Rotate lift down to get specimen close to bar
if( opModeIsActive() ) {
robot.geckoServo.setPower(-0.50); // hold it while we clip
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down Expand Up @@ -273,7 +273,7 @@ private void scoreSpecimenPreload() {

// Lower the arm for parking
if( opModeIsActive() ) {
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down Expand Up @@ -312,7 +312,7 @@ private void level1Ascent() {

if( opModeIsActive() ) {
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB);
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_BASKET);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_DEG);
timeDriveStraight(-DRIVE_SPEED_20,3000);
do {
if( !opModeIsActive() ) break;
Expand Down
8 changes: 4 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousLeftRed.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,7 +189,7 @@ private void scoreSpecimenPreload() {
// Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!)
pos_y=3.0; pos_x=0.0; pos_angle=0.0; // start at this absolute location
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO1);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG);
pos_y += 3.0;
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1);
Expand Down Expand Up @@ -228,7 +228,7 @@ private void scoreSpecimenPreload() {
// Rotate lift down to get specimen close to bar
if( opModeIsActive() ) {
robot.geckoServo.setPower(-0.50); // hold it while we clip
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down Expand Up @@ -273,7 +273,7 @@ private void scoreSpecimenPreload() {

// Lower the arm for parking
if( opModeIsActive() ) {
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down Expand Up @@ -312,7 +312,7 @@ private void level1Ascent() {

if( opModeIsActive() ) {
autoViperMotorMoveToTarget( robot.VIPER_EXTEND_GRAB);
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_BASKET);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_BASKET_DEG);
timeDriveStraight(-DRIVE_SPEED_20,3000);
do {
if( !opModeIsActive() ) break;
Expand Down
6 changes: 3 additions & 3 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 @@ -199,7 +199,7 @@ private void scoreSpecimenPreload() {
// Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!)
pos_y=3.0; pos_x=0.0; pos_angle=0.0; // start at this absolute location
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO1);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG);
pos_y += 3.0;
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1);
Expand Down Expand Up @@ -238,7 +238,7 @@ private void scoreSpecimenPreload() {
// Rotate lift down to get specimen close to bar
if( opModeIsActive() ) {
robot.geckoServo.setPower(-0.50); // hold it while we clip
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down Expand Up @@ -283,7 +283,7 @@ private void scoreSpecimenPreload() {

// Lower the arm for parking
if( opModeIsActive() ) {
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down
6 changes: 3 additions & 3 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 @@ -199,7 +199,7 @@ private void scoreSpecimenPreload() {
// Move away from field wall (viper slide motor will hit field wall if we tilt up too soon!)
pos_y=3.0; pos_x=0.0; pos_angle=0.0; // start at this absolute location
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_50, TURN_SPEED_20, DRIVE_THRU );
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO1);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO1_DEG);
pos_y += 3.0;
driveToPosition( pos_y, pos_x, pos_angle, DRIVE_SPEED_70, TURN_SPEED_20, DRIVE_THRU );
robot.elbowServo.setPosition(robot.ELBOW_SERVO_BAR1);
Expand Down Expand Up @@ -238,7 +238,7 @@ private void scoreSpecimenPreload() {
// Rotate lift down to get specimen close to bar
if( opModeIsActive() ) {
robot.geckoServo.setPower(-0.50); // hold it while we clip
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_AUTO2);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_AUTO2_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down Expand Up @@ -283,7 +283,7 @@ private void scoreSpecimenPreload() {

// Lower the arm for parking
if( opModeIsActive() ) {
autoTiltMotorMoveToTarget( robot.TILT_ANGLE_ZERO);
autoTiltMotorMoveToTarget(Hardware2025Bot.TILT_ANGLE_ZERO_DEG);
do {
if( !opModeIsActive() ) break;
// wait for lift/tilt to finish...
Expand Down
26 changes: 13 additions & 13 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 @@ -33,8 +33,8 @@ public class Hardware2025Bot

//====== INERTIAL MEASUREMENT UNIT (IMU) =====
protected BNO055IMU imu = null;
public double headingAngle = 0.0;
public double tiltAngle = 0.0;
public double imuHeadingAngle = 0.0;
public double imuTiltAngle = 0.0;

//====== GOBILDA PINPOINT ODOMETRY COMPUTER ======
GoBildaPinpointDriver odom;
Expand Down Expand Up @@ -99,9 +99,9 @@ public class Hardware2025Bot
public double wormTiltMotorSetPwr = 0.0; // requested power setting
public double wormTiltMotorPwr = 0.0; // current power setting

protected AnalogInput turretEncoder = null; // US Digital absolute magnetic encoder (MA3)
public double turretAngle = 0.0; // 0V = 0 degrees; 3.3V = 359.99 degrees
public double turretAngleOffset = 129.0; // allows us to adjust the 0-360 deg range
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 int TILT_ANGLE_HW_MAX = 3675; // encoder at maximum rotation UP/BACK (horizontal = -200)
Expand All @@ -115,7 +115,7 @@ public class Hardware2025Bot
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 double startingTurretAngle = 97.0;
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
Expand Down Expand Up @@ -268,8 +268,8 @@ public void init(HardwareMap ahwMap, boolean isAutonomous ) {
// Define and Initialize pan and tilt motors
wormPanMotor = hwMap.get(DcMotorEx.class,"WormPan"); // Control Hub port 0
wormTiltMotor = hwMap.get(DcMotorEx.class,"WormTilt"); // Control Hub port 1
turretEncoder = hwMap.get(AnalogInput.class, "tiltMA3"); // Expansion Hub analog 0
startingTurretAngle = computeAbsoluteAngle( turretEncoder.getVoltage(), turretAngleOffset );
armTiltEncoder = hwMap.get(AnalogInput.class, "tiltMA3"); // Expansion Hub analog 0
startingTurretAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset);

wormPanMotor.setDirection(DcMotor.Direction.FORWARD);
wormTiltMotor.setDirection(DcMotor.Direction.FORWARD);
Expand Down Expand Up @@ -351,9 +351,9 @@ public void initIMU()
public double headingIMU()
{
Orientation angles = imu.getAngularOrientation( AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES );
headingAngle = angles.firstAngle;
tiltAngle = angles.secondAngle;
return -headingAngle; // degrees (+90 is CW; -90 is CCW)
imuHeadingAngle = angles.firstAngle;
imuTiltAngle = angles.secondAngle;
return -imuHeadingAngle; // degrees (+90 is CW; -90 is CCW)
} // headingIMU

/*--------------------------------------------------------------------------------------------*/
Expand Down Expand Up @@ -400,7 +400,7 @@ public double computeAbsoluteAngle( double measuredVoltage, double zeroAngleOffs
/* using this function to account for that offset, we can place zero where we want it in s/w. */
/* Having DEGREES_PER_ROTATION as a variable lets us adjust for the 3.3V vs. 5.0V difference. */
/*--------------------------------------------------------------------------------------------*/
public int computeEncoderCountsFromAngle( double angle )
public static int computeEncoderCountsFromAngle( double angle )
{
return (int)((startingTurretAngle - angle) * ENCODER_COUNTS_PER_DEG);
} // computeEncoderCountsFromAngle
Expand Down Expand Up @@ -433,7 +433,7 @@ public void readBulkData() {
wormTiltMotorPos = wormTiltMotor.getCurrentPosition();
wormTiltMotorVel = wormTiltMotor.getVelocity();
wormTiltMotorPwr = wormTiltMotor.getPower();
turretAngle = computeAbsoluteAngle( turretEncoder.getVoltage(), turretAngleOffset );
armTiltAngle = computeAbsoluteAngle( armTiltEncoder.getVoltage(), armTiltAngleOffset);
// NOTE: motor mA data is NOT part of the bulk-read, so increases cycle time!
// frontLeftMotorAmps = frontLeftMotor.getCurrent(MILLIAMPS);
// frontRightMotorAmps = frontRightMotor.getCurrent(MILLIAMPS);
Expand Down
16 changes: 9 additions & 7 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.turretAngle);
telemetry.addData("Tilt", "%d counts %.1f deg", robot.wormTiltMotorPos, robot.armTiltAngle);
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 @@ -591,8 +591,10 @@ else if(panAngleTweaked) {

/*---------------------------------------------------------------------------------*/
void processTiltControls() {
boolean safeToManuallyLower = (robot.wormTiltMotorPos > robot.TILT_ANGLE_HW_MIN);
boolean safeToManuallyRaise = (robot.wormTiltMotorPos < robot.TILT_ANGLE_HW_MAX);
// 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);
double gamepad2_right_stick = gamepad2.right_stick_y;
boolean manual_tilt_control = ( Math.abs(gamepad2_right_stick) > 0.08 );

Expand Down Expand Up @@ -662,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.viperMotorPos > robot.TILT_ANGLE_RAISED)? 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 @@ -784,7 +786,7 @@ void processLevel2Ascent() {
break;
case ASCENT_STATE_SETUP:
// Send TILT motor to hang position
robot.wormTiltMotor.setTargetPosition( robot.TILT_ANGLE_HANG1 );
robot.wormTiltMotor.setTargetPosition( robot.computeEncoderCountsFromAngle(Hardware2025Bot.TILT_ANGLE_HANG1_DEG) );
robot.wormTiltMotor.setMode( DcMotor.RunMode.RUN_TO_POSITION );
robot.wormTiltMotor.setPower( 0.90 );
// Send LIFT motor to hang position
Expand All @@ -794,7 +796,7 @@ void processLevel2Ascent() {
ascent2state = ASCENT_STATE_MOVING;
break;
case ASCENT_STATE_MOVING :
boolean tiltReady = (Math.abs(robot.wormTiltMotorPos - robot.TILT_ANGLE_HANG1) < 20)? true:false;
boolean tiltReady = (Math.abs(robot.armTiltAngle - robot.TILT_ANGLE_HANG1_DEG) < 0.2)? true:false;
boolean viperReady = (Math.abs(robot.viperMotorPos - robot.VIPER_EXTEND_HANG1) < 20)? true:false;
break;
case ASCENT_STATE_READY :
Expand Down Expand Up @@ -822,7 +824,7 @@ void processLevel2Ascent() {
robot.viperMotor.setPower( -0.10 ); // hold power
}
// Do we need to further store the robot drivetrain?
boolean tiltRetractionIncomplete = (robot.wormTiltMotorPos < robot.TILT_ANGLE_HANG2)? true : false;
boolean tiltRetractionIncomplete = (robot.armTiltAngle > robot.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 31068f4

Please sign in to comment.