Quote:
|
Originally Posted by robotaddict
Could someone out there tell me if I got the tail anywhere near the donkey with these equations? 
|
Sorry, I don't fully understand where you're going, but that doesn't mean its wrong. In the algorithm I posted above there are a few things I need to clarify. The code uses our implementation of math.c, which you can download here:
math.zip. The zip file includes the math header and source files as well as our type library. Its much easier and less confusing to use "uint16" for an unsigned 16-bit integer than "unsigned int".
The sind() and cosd() functions return the sine and cosine of an angle measured in integer degrees, between 0 and 359, as a fixed-point decimal (FPD). That just means that its an integer where the lowest 10 bits are the decimal. sind() and cosd() are implemented using a lookup table of sine values for angles 0-89, where the cosine is derived from the sine. atand() is implemented as a binary search through the arctangent values of angles 0 to 90. The square root implementation is based on a algorithm I found on Microchip's web site.
You can add and subtract FPD values normally, but multiplication and division are different. Multiplication causes the decimal in the result to be as long as the sum of the decimals in the two operands. So multiplying two FPD with 10 bits of precision will result in a number with a 20 bit decimal. Division results in a decimal with the difference between the two operands, so a 10 bit decimal divided by a 10 bit decimal results in an integer. The functions mulf() and divf() handle multiplication and division for 10-bit FPD values. The funtion itof() converts an integer to a FPD and ftoi() converts a FPD back to an integer. A side effect is that multiplying an integer by a FPD results in a FPD without having to shift the result, because the product will have (0 + 10) = 10 bits of precision.
Anyways, these source files may help you make sense of the code I posted up above. If you have a spare robot controller you can try implementing the code without hooking up any motors and view the results as printf's or with a dashboard viewer. For now, just ignore the gyro integrator and set the heading, phi, to 0 all the time. Heres what happens when phi is 0:
sinPhi = 0
cosPhi = 1
x2 = (cosPhi * x) + (sinPhi * y) = x;
y2 = -(sinPhi * x) + (cosPhi * y) = y;
vNorth = x2 - r = x - r;
vSouth = -x2 - r = -x - r;
vEast = -y2 - r = -y - r;
vWest = y2 - r = y - r;
Assume you're using a joystick on port 1 for your X and Y movement (forward/backward and strafe left/right) and port 2 for your rotational component.
x = p1_x;
y = p1_y;
r = p2_x;
CASE 1: DRIVE FORWARD/BACKWARD
--------------------------------------------------------
If you want to drive straight forward or backward the inputs and outputs are as follows:
x = 127 - 127 = 0; y = 254 - 127 = 127; r = 127 - 127 = 0;
VNorth = x - r = 0;
VSouth = -x - r = 0;
VEast = -y - r = -127;
VWest = y - r = 127;
PWM_North = VNorth + 127 = 127;
PWM_South = VSouth + 127 = 127;
PWM_East = VEast + 127 = 0;
PWM_West = VWest + 127 = 254;
So, as expected, you'll be driving only the EAST and WEST wheels.
CASE 2: STRAFE RIGHT AND LEFT
--------------------------------------------------------
If you want to strafe left or right the inputs and outputs are as follows:
x = 255 - 127 = 0; y = 127 - 127 = 127; r = 127 - 127 = 0;
VNorth = x - r = 127;
VSouth = -x - r = -127;
VEast = -y - r = 0;
VWest = y - r = 0;
PWM_North = VNorth + 127 = 254;
PWM_South = VSouth + 127 = 0;
PWM_East = VEast + 127 = 127;
PWM_West = VWest + 127 = 127;
So, as expected, you'll be driving only the NORTH and SOUTH wheels.
CASE 3: ROTATE IN PLACE
--------------------------------------------------------
If you want to rotate in place, the inputs are as follows:
x = 127 - 127 = 0; y = 127 - 127 = 127; r = 254 - 127 = 127;
VNorth = x - r = -127;
VSouth = -x - r = -127;
VEast = -y - r = -127;
VWest = y - r = -127;
PWM_North = VNorth + 127 = 0;
PWM_South = VSouth + 127 = 0;
PWM_East = VEast + 127 = 0;
PWM_West = VWest + 127 = 0;
So, as expected, you'll be driving all four wheels in the same direction.
--------------------------------------------------------
If you use a gyro to integrate your current heading and apply that value as angle phi, then the control method will completely change. The robot will no longer have a "front", rather the robot will move with repect to the driver. Push the joystick away and the robot moves away, no matter which direction its facing. Push the joystick away and apply some rotation and it will "frisbee" (stole that term from Rich LeGrand) away from you.
If you apply the simple feedback loop I mentioned in my previous post, you would use the adjusted r value when you set the values of VNorth, VSouth, VEast, and VWest. In order for this system to really work well you would need to have precise control over the wheel velocities. The wheels on our robot are not evenly spaced, so the extra drag on one side makes it very difficult to drive in a straight line. Good luck.