Quote:
Originally Posted by Ether
Please post a counter-example showing what doesn't work with the code I posted. If there's a problem with it, I want to know.
|
I crunched both examples and got 360. maybe my calculator got it wrong, but I think the floor is the issue since it will gives either 0 or 1 and thus either 0 or 360.
I attached my code.
how my code works:
1. each wheel is its own vector, with right being 0 radians like a coordinate plane. therefore, forward = pi/2, left= pi, and backward = 3pi/2
2. every angle is in radians.
3. the encoder values are divided by 5 and multiplied by 2pi to make them radian encoders. I then subtract a calibration value and then use sin and asin to make all values between 0 and 2pi.
4. x and y are already broken down components. z (rotate) is not. z is rendered by taking the optimal angle for each wheel to turn the robot clockwise (can be found with trig, my values are for approx what my unibot has), lets call it Q for now, and making Xz= cos(Q)*z and Yz = sin(Q)*z
5. to generate one wheel's final target vector, x+Xz=Xf ; y+Yz=Yf. then, to find the vector's magnitude, we use the distance formula on Xf and Yf. The direction is found using atan2 on Xf and Yf.
6. the vector is then adjusted for negative magnitude benefits. I figured if the distance between target postion (TP) and current position (CP) is between pi/2 and 3pi/2, then subtracting (+ distance) or adding (- distance) pi from the direction and changing the sign on the magnitude will increase agility.
7. Using the modified vector, the new TP distance from CP is calcualted and then put though a conditional section that takes the original distance (OD) and then adds/subtracts 2pi (same sign rules as #6) to make modified distance (MD). then, it finds which one is closer to zero and sets it as the new PID setpoint. the PID's process variable is always 0. the magnitude is sent as a drive motor %vbus.
8. It does #3-#7 for each wheel independently.
it probably could be simplified in the steps in #6 and #7... the reverse magnitude bit was added first... this is just to be modular enough so the next time i can test the code, I can follow each step.
questions?