Skip to content

Commit

Permalink
added field centric code to ChickalettaHardware
Browse files Browse the repository at this point in the history
  • Loading branch information
@narehhovsepyan
narehhovsepyan committed Nov 18, 2023
1 parent 41150bc commit 3136461
Show file tree
Hide file tree
Showing 2 changed files with 87 additions and 44 deletions.
75 changes: 59 additions & 16 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ChickalettaHardware.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,8 @@

// import com.qualcomm.hardware.motors.RevRoboticsCoreHexMotor;

import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
Expand All @@ -28,7 +29,7 @@ public class ChickalettaHardware {
private DcMotor spinTake = null;

private DcMotor arm = null;
private BNO055IMU imu = null;
private IMU imu = null;
private double robotHeading = 0;
private double headingOffset = 0;
private double headingError = 0;
Expand All @@ -41,9 +42,9 @@ public class ChickalettaHardware {
static final double DRIVE_SPEED = 0.6;
static final double TURN_SPEED = 1.0;
private double turnSpeed = 0;
static final double P_TURN_GAIN = 0.008; // Larger is more responsive, but also less stable
static final double P_DRIVE_GAIN = 0.02; // Larger is more responsive, but also less stable
static final double HEADING_THRESHOLD = 5.0 ;
static final double P_TURN_GAIN = 0.008; // Larger is more responsive, but also less stable
static final double P_DRIVE_GAIN = 0.02; // Larger is more responsive, but also less stable
static final double HEADING_THRESHOLD = 5.0;


// Define Drive constants. Make them public so they CAN be used by the calling OpMode
Expand Down Expand Up @@ -77,10 +78,16 @@ public void init() {
leftBackDrive.setDirection(DcMotor.Direction.FORWARD);
rightFrontDrive.setDirection(DcMotor.Direction.REVERSE);
rightBackDrive.setDirection(DcMotor.Direction.FORWARD);
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
imu = myOpMode.hardwareMap.get(BNO055IMU.class, "imu");

// Retrieve the IMU from the hardware map
imu = myOpMode.hardwareMap.get(IMU.class, "imu");
// Adjust the orientation parameters to match your robot
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
// Without this, the REV Hub's orientation is assumed to be logo up / USB forward
imu.initialize(parameters);

resetHeading();
myOpMode.telemetry.addData(">", "Hardware Initialized");
myOpMode.telemetry.update();
Expand Down Expand Up @@ -160,10 +167,10 @@ public void strafeTimed(double lateral, double time) {
stop();
}


public void driveRobot(double axial, double lateral, double yaw) {
double max;


// Combine the joystick requests for each axis-motion to determine each wheel's power.
// Set up a variable for each drive wheel to save the power level for telemetry.
double leftFrontPower = axial + lateral + yaw;
Expand Down Expand Up @@ -192,9 +199,35 @@ public void driveRobot(double axial, double lateral, double yaw) {
myOpMode.telemetry.addData("Back left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
}

public void driveRobotFC(double axial, double lateral, double yaw) {
double y = axial;
double x = lateral;
double rx = yaw;

double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);

// Rotate the movement direction counter to the bot's rotation
double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading);
double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading);

rotX = rotX * 1.1; // Counteract imperfect strafing

// Denominator is the largest motor power (absolute value) or 1
// This ensures all the powers maintain the same ratio,
// but only if at least one is out of the range [-1, 1]
double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1);
double frontLeftPower = (rotY + rotX + rx) / denominator;
double backLeftPower = (rotY - rotX + rx) / denominator;
double frontRightPower = (rotY - rotX - rx) / denominator;
double backRightPower = (rotY + rotX - rx) / denominator;

leftFrontDrive.setPower(frontLeftPower);
leftBackDrive.setPower(backLeftPower);
rightFrontDrive.setPower(frontRightPower);
rightBackDrive.setPower(backRightPower);
}

public void arm(double arm_power) {
public void arm(double arm_power) {
arm.setPower(arm_power);
myOpMode.telemetry.addData("arm", "%4.2f", arm_power);
}
Expand All @@ -213,12 +246,19 @@ public void setMotorMode(DcMotor.RunMode motorMode) {
}

public double getRawHeading() {
Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
return angles.firstAngle;}
return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
}

public void resetHeading() {
// Save a new heading offset equal to the current raw heading.
headingOffset = getRawHeading();
robotHeading = 0;}
robotHeading = 0;
}

public void resetYaw() {
imu.resetYaw();
}

public double getSteeringCorrection(double desiredHeading, double proportionalGain) {
targetHeading = desiredHeading; // Save for telemetry
// Get the robot heading by applying an offset to the IMU heading
Expand All @@ -229,7 +269,9 @@ public double getSteeringCorrection(double desiredHeading, double proportionalGa
while (headingError > 180) headingError -= 360;
while (headingError <= -180) headingError += 360;
// Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0
return Range.clip(headingError * proportionalGain, -1, 1);}
return Range.clip(headingError * proportionalGain, -1, 1);
}

public void turnToHeading(double maxTurnSpeed, double heading) {
getSteeringCorrection(heading, P_DRIVE_GAIN);
while (myOpMode.opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) {
Expand All @@ -238,13 +280,14 @@ public void turnToHeading(double maxTurnSpeed, double heading) {
// Clip the speed to the maximum permitted value.
turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
// Pivot in place by applying the turning correction
driveRobot(0, 0, -turnSpeed);}
driveRobot(0, 0, -turnSpeed);
}
stop();
}

public void stop() {
driveRobot(0, 0, 0);
// slide(0);
// slide(0);
}

public void straight(double power) {
Expand Down
56 changes: 28 additions & 28 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ChickalettaTeleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,39 +39,40 @@
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When a selection is made from the menu, the corresponding OpMode is executed.
*
* <p>
* This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot.
* This code will work with either a Mecanum-Drive or an X-Drive train.
* Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html
* Note that a Mecanum drive must display an X roller-pattern when viewed from above.
*
* <p>
* Also note that it is critical to set the correct rotation direction for each motor. See details below.
*
* <p>
* Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously.
* Each motion axis is controlled by one Joystick axis.
*
* <p>
* 1) Axial: Driving forward and backward Left-joystick Forward/Backward
* 2) Lateral: Strafing right and left Left-joystick Right and Left
* 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left
*
* <p>
* This code is written assuming that the right-side motors need to be reversed for the robot to drive forward.
* When you first test your robot, if it moves backward when you push the left stick forward, then you must flip
* the direction of all 4 motors (see code below).
*
* <p>
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/

@TeleOp(name="Teleop 2024", group="Linear Opmode")
@TeleOp(name = "Teleop 2024", group = "Linear Opmode")
//@Disabled
public class ChickalettaTeleop extends LinearOpMode {

// Declare OpMode members for each of the 4 motors.
ChickalettaHardware robot = new ChickalettaHardware(this);
ChickalettaHardware robot = new ChickalettaHardware(this);
private final ElapsedTime runtime = new ElapsedTime();
// @Override

// @Override
public void runOpMode() {
robot.init();
robot.init();

telemetry.addData("Status", "Initialized");
telemetry.update();
Expand All @@ -82,36 +83,39 @@ public void runOpMode() {
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
double max;

double gp1LY=gamepad1.left_stick_y;
double gp1LX=gamepad1.left_stick_x;
double gp1RX=gamepad1.right_stick_x;
double slowscale=0.33333333333;
if (gamepad1.right_bumper){
// This button choice was made so that it is hard to hit on accident,
// it can be freely changed based on preference.
// The equivalent button is start on Xbox-style controllers.
if (gamepad1.options) {
robot.resetYaw();
}
double gp1LY = gamepad1.left_stick_y;
double gp1LX = gamepad1.left_stick_x;
double gp1RX = gamepad1.right_stick_x;
double slowscale = 0.33333333333;
if (gamepad1.right_bumper) {
gp1LY *= slowscale;
gp1LX *= slowscale;
gp1RX *= slowscale;
}
robot.driveRobot(-gp1LY, gp1LX, gp1RX);
robot.driveRobotFC(-gp1LY, gp1LX, gp1RX);

// If the slide goes the wrong way, change the negative sign!!!!!!!
double arm_position;
if (gamepad2.dpad_down) {
arm_position = 1.0;
}
else if (gamepad2.dpad_up) {
} else if (gamepad2.dpad_up) {
arm_position = -1.0;
} else {
arm_position = 0.0;
}
robot.arm(arm_position);
}
}

double intake_position;
if (gamepad1.dpad_down) {
intake_position= 1.0;
}
else if (gamepad1.dpad_up) {
intake_position = 1.0;
} else if (gamepad1.dpad_up) {
intake_position = -1.0;
} else {
intake_position = 0.0;
Expand All @@ -120,8 +124,4 @@ else if (gamepad1.dpad_up) {
}






}
}

0 comments on commit 3136461

Please sign in to comment.