search

Team2168 / 2022_Main_Robot

Other
3 stars 0 forks source link

pose estimation to figure out shooting while moving #81

Open Shubbii opened 2 years ago

Shubbii commented 2 years ago

Figure out the standard deviations for the drivetrain encoders and limelight vision Learn how to actually use the DifferentialDrivePoseEstimator class understand how to actually calculate our shooter speed and hood angle while moving and put it into code

jcorcoran commented 2 years ago

Steps that might-maybe-possibly let us shoot while moving, in an order that makes sense to me:

  1. Use odometry to calculate the Robot relative goal position (herein referred to as BOB cause I don't want to keep writing out Robot relative goal position) BOB is the angle at which the center of the Hub resides relative to robot's forward (0.0 degrees) heading. Or in other words, the angle to which the turret should be commanded to face the shooter at the hub
  2. Use the camera to calculate BOB This should be possible (when a target is in view) by subtracting the visible target position, tx, from the current turret angle
  3. Fuse the camera derived BOB with the odometry derived BOB together. A kalman filter may be appropriate: (https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/math/estimator/KalmanFilter.html)[javadoc]
  4. Write code to aim the turret at the fused BOB
  5. Calculate the robot's field relative velocity vector
    • Derive the toward goal component of the velocity vector
    • Derive the across goal component of the velocity vector
  6. Adjust the turret position to counter the across goal component of the velocity vector
    • If the robot's across goal velocity vector is traveling to the left of the hub, we will need to aim to the right of the hub's center to actually make the ball in (since it's imparted with a velocity component that will cause it to continue to travel to the left as it flies through the air)
  7. Adjust the hood/shooter speed to counter the towards goal component of the velocity vector
    • If the robot's towards goal velocity component is moving away from the goal, we will need to increase the shooter speed (as though we were shooting from farther away), since the robot's velocity vector is effectively subtracting from the velocity the shooter is imparting into the ball towards the goal.

^ This is a guess at an approach that might work. we will likely get smarter and revise the above as work on this problem is performed