Hi. I found a code on github, that uses odometry, and has an auto-aim ability. I am wondering how I can implement this into my own code. I already have a working odom, but I am struggling with the auto aim part. I took the part from the github code, but I want to understand it and make it work before I redo it, because I do not copy code for competitions. I will attach a photo that will have the code. All I am trying to figure out is if I make it work like any other function, with it working with pressed(), and I also am trying to figure out what aac is and what I have to do with it. Thank you for the help!

void AutoAim(){
    double aac;
    double YendOfField = 135;
    double XendofField = X;
    double Xgoal = 15;
    double Ygoal = 119;
    double lengthOfArc1;
    double lengthOfArc2;
    double lengthOfArc3;
    double angleToGoal;
    double robotToGoal;
    
    lengthOfArc1 = 135 - Y; //a
    lengthOfArc2 = sqrt((X - Xgoal)*(X - Xgoal)+(Y - Ygoal)*(Y - Ygoal)); //b
    lengthOfArc3 = sqrt((Xgoal - XendofField)*(Xgoal - XendofField)+(Ygoal - YendOfField)*(Ygoal - YendOfField)); //c

    angleToGoal = acos((lengthOfArc2)*(lengthOfArc2)+(lengthOfArc3)*(lengthOfArc3)-(lengthOfArc1)*(lengthOfArc1)/(2*lengthOfArc2)*(lengthOfArc3));
    robotToGoal = angleToGoal - OdomHeading;

  std::cout<<lengthOfArc1;
  std::cout<<lengthOfArc2;
  std::cout<<lengthOfArc3;
  }

As long as you know where the goal is on your coordinate system, you can use atan2 to get the angle to the goal and use your turn function to spin to it. You can look at this stackoverflow answer for more info

This is meant for driver Ctrl. Not sure if that helps at all

The math would be the same, but I would suggest having some way of doing it that would be less intrusive to your driver then disabling the controls to run a turn PID. How I would probably do it is when the aim button is pressed, start a turn PID in a second thread and disable drivetrain controls in the main thread, and make the aim button stop the turn and re-enable drivetrain controls. This way the driver can press the aim button, but if the turn gets stuck they can just press the button again to stop aiming.

Also, I’m not sure if aiming during driver control is the best idea. It could work fine in skills, but in a normal match the odometry would probably get knocked off from robots hitting each other.