Odom2 #8
Odom2 #8
Conversation
|
|
||
| //Updates the Odometry Robot location | ||
| void | ||
| Odometry::updateOdometry(double BR_A, double BR_V, |
There was a problem hiding this comment.
A rewrite of the odometry class is the major change.
| m_chooser.AddOption("RED", redAlliance); | ||
| frc::SmartDashboard::PutData("Alliance Color", &m_chooser); | ||
|
|
||
| frc::CameraServer::StartAutomaticCapture(); |
There was a problem hiding this comment.
Don't use GetInstance, it has a deprecation warning.
|
|
||
| //This might not work?? | ||
| m_climber.Initialize(); | ||
| double Robot::GetGyroAngleRad() { |
| m_intake.Periodic(); | ||
| m_shooter.Periodic(m_swerve.GetXPosition(), m_swerve.GetYPosition()); | ||
| void Robot::RobotPeriodic() { | ||
| m_swerve.UpdateOdometry(GetGyroAngleRad(), m_timeStep); |
There was a problem hiding this comment.
Call update odom in RobotPeriodic instead of only when following trajectories, this sets you up for shooter compensation later.
| double x_off = m_limelight->getXOff()+3.85; | ||
| if(x_off == 10000.0 && m_OdometryErrorRate < 10){ | ||
| //tan of distance between hub and robot | ||
| double hubX = m_lastDistance*cos(m_lastAngle/PI*180) |
There was a problem hiding this comment.
I had to change this area because it didn't compile before, no semicolons and mismatched parens.
|
|
||
| // Updates the Odometry | ||
| void SwerveDrive::UpdateOdometry(double theta_rad, double dt) { | ||
| // NOTE I am explicitly ignoring any polarity flips, |
There was a problem hiding this comment.
This is the only substantial change to the swerve, make close note of this comment btw
|
|
||
| bool YError = (abs(GetYPosition() - m_trajectory_1.getY(index)) < 0.3); | ||
| bool XError = (abs(GetXPostion() - m_trajectory_1.getX(index)) < 0.3); |
There was a problem hiding this comment.
Method rename, code wasn't compiling because it mixed GetXPostion and GetXPosition
| double | ||
| WheelDrive::getVelocity(){ | ||
| return speedMotor.GetSelectedSensorVelocity()/2048 * (1/6.12) * 4.375 * PI / 5/ 12; | ||
| double WheelDrive::getWheelDistance() { |
There was a problem hiding this comment.
Only real change here is adding this supporting method
| double value = getAngle(offSet); | ||
| if(abs(value - angle) > 90){ | ||
| angle = (angle > 0 )? angle - 180: angle + 180; | ||
| if (abs(value - angle) > 90) // TODO: this doesn't handle wrapping behavior |
There was a problem hiding this comment.
Be sure to revisit this code to handle angle wrapping behavior for a more robust swerve.
Imagine value is 359 (or -179) and angle is 1 (or 179), this code wouldn't properly measure the distance between those angles.
| const int r_joy_port = 1; | ||
| const int O_joy_port = 2; | ||
| /* | ||
| Coordinate Frames: |
There was a problem hiding this comment.
Specifying the coordinate frame standards I'm using
| const double rotI = 0.00006; | ||
| const double rotD = 0.0; | ||
|
|
||
| const double EMPIRIC_WHEEL_METERS_PER_TICK = 1.0; // TODO: TUNE |
There was a problem hiding this comment.
You need to tune this: setup the robot in teleop with all four swerve wheel encoder values on smartdashboard.
Drive in a straight line with no rotation. Measure the distance the bot traveled in the real world. Take the difference between the before and after encoder values on smart dashboard. Average all four differences, and then divide the number of meters traveled by that average.
| DriveConstants::BROFF); | ||
| m_backLeft.drive(backLeftSpeed, backLeftAngle, | ||
| DriveConstants::BLOFF); | ||
| m_backRight.drive(-backRightSpeed, backRightAngle, DriveConstants::BROFF); |
There was a problem hiding this comment.
These speed shouldn't need to be inverted, this indicates your modules are 180deg out of tune.
There was a problem hiding this comment.
Per my definition of tuned: 0rad, positive power, drives the robot forward (+x direction in robot frame)
Unfortunately the state of the repo left a lot of noise in this diff. I'll try to highlight the actual changes so they stand out from what my autoformatter did. I probably should have turned it off, but it's too late to decouple things now.