Hello,
1st question: Autonomous mode:
We just swtiched from using the Expansion HUB IMU BNO055 to the Control Hub IMU BHI260AP.
We apadted our program to update the class initializations etc and our previous Autonomous code works (now considering the new IMU class).
Question is: are there any recommended tests we can perform to make sure the IMU class is initialized correctly?
2nd question: TeleOp Mode
We are trying to incorporate the FTC sample code for our TeleOp (RobotHardware and adapting ConceptExternalHardwareClass
However, our strafing is not working.:
Here’s the code snippets so far:
In RobotHardware:
RF = myOpMode.hardwareMap.get(DcMotor.class, “RF”);
LF = myOpMode.hardwareMap.get(DcMotor.class, “LF”);
RB = myOpMode.hardwareMap.get(DcMotor.class, “RB”);
LB = myOpMode.hardwareMap.get(DcMotor.class, “LB”);
public void driveRobot(double axial, double lateral, double yaw) {
// 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;
double rightFrontPower = axial + lateral - yaw;
double leftBackPower = axial + lateral + yaw;
double rightBackPower = axial - lateral - yaw;
// Normalize the values so no wheel power exceeds 100%
// This ensures that the robot maintains the desired motion.
double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
max = Math.max(max, Math.abs(leftBackPower));
max = Math.max(max, Math.abs(rightBackPower));
if (max > 1.0) {
leftFrontPower /= max;
rightFrontPower /= max;
leftBackPower /= max;
rightBackPower /= max;
}
// Send calculated power to wheels
LF.setPower(leftFrontPower);
RF.setPower(rightFrontPower);
LB.setPower(leftBackPower);
RB.setPower(rightBackPower);
}
In the main TeleOp Class we have such that left_stick_x should strafe:
axial = -gamepad1.left_stick_y; // driving forward and backward Left-joystick Forward/Backward
lateral = gamepad1.left_stick_x; //Strafing right and left Left-joystick Right and Left
yaw = gamepad1.right_stick_x; //Rotating Clockwise and counter clockwise Right-joystick Right and Left
and then call:
robot.driveRobot(axial, lateral, yaw);
The drive froward/backward and the turning left/right works but the strafing doesnt. May I missing anything?
Thanks for your help
Prasad
Question 1: When you say “is initialized correctly”, do you mean “is the Hub orientation defined correctly”?
This can be tested with a simple standalone OpMode that displays live Telemetry of Yaw, Pitch and Roll. Slowly rotate the Hub (or entire robot) manually through a small angle, roughly isolating only one axis of rotation. Look for rotation values that change in accordance with the robot axes, using the right-hand rule.
More info is here:
http://ftc-docs.firstinspires.org/programming_resources/imu/imu.html
Question 2: You say the strafing “doesn’t work”. Is the problem simply that the robot strafes left when LeftStickX moves right? If so, this is because your code seems to make the left-side wheels rotate towards each other with positive values of lateral or LeftStickX (to the right). You want those wheels to rotate away from each other, assuming your mecanum rollers are arranged in the customary X pattern.
On the right side, you want the wheels to rotate towards each other, when strafing right. Your code seems to do the opposite.
So, try flipping the signs for lateral.
Deeper troubleshooting might involve testing each motor one at a time, to verify the power cables are connected to the intended/named port, that encoders are connected to the corresponding motor port, and that the motors are rotating in the directions expected. You didn’t post the Motor Direction settings, but those could be checked also.
Here are the motor direction settings:
LF.setDirection(DcMotorSimple.Direction.FORWARD);
RF.setDirection(DcMotorSimple.Direction.REVERSE);
LB.setDirection(DcMotorSimple.Direction.FORWARD);
RB.setDirection(DcMotorSimple.Direction.REVERSE);
When strafing didnt work I meant to say there is no movement for the robot when I use the LeftStickX and move it laterally left or right.
assume it has to do with these equations:
double leftFrontPower = axial - lateral + yaw;
double rightFrontPower = axial + lateral - yaw;
double leftBackPower = axial + lateral + yaw;
double rightBackPower = axial - lateral - yaw;
And based on your above comment, you are recommending to try this:
double leftFrontPower = axial + lateral + yaw;
double rightFrontPower = axial - lateral - yaw;
double leftBackPower = axial - lateral + yaw;
double rightBackPower = axial + lateral - yaw;
You said “no movement for the robot”.
If this means the wheels do not move, make sure your code says gamepad1.left_stick_x, not gamepad2.left_stick_x (or something else).
If the wheels are turning but the robot is not strafing, then your left and right sides are both rotating towards each other (for one strafing input), and away from each other (for the other strafing input). The vectors cancel, and there is no net sideways motion.
I would suggest again that you test one motor at a time, to be sure that the configured names and port numbers agree with the physical position on the robot (and the encoder cables). Having them wrong could still allow straight driving and spinning to work (essentially tank drive), which you reported.