pic: kiwi vex robot

[Joel J]

Originally Posted by billbo911

Now I know why I like Tom (oh yeah, and Joe too ) so much!!

The problem is, now I have to go buy some omni wheels for this weekends project

BTW. Do you know how the constant values were determined (ie. 37, 359, 601, 168, 97, 161)? Also, I see you are using PWM_in4 for the rotation value. I assume this is off the left stick X-axis. Knowing these can help me modify this stuff for future projects.

Thanks again!!!!

Here's the "documentation." Its in the PBASIC file that I used to program the KIWI drive 229 did a year or so ago.

'KIWI Code Overview
'------------------
'
'Vx, Vy, and w are on the interval [-127, 127], but the inputs p2_x, p2_y, p1_x are on the interval [0, 254], 
'so we compensate by setting Vx, Vy, and w to the shifted version of the input:
'
'	trans_x = p2_x
'	trans_y = p2_y
'	rot 	= p1_x
'
'	Vx = trans_x - 127
'	Vy = trans_y - 127
'	w  = rot - 127
'
'The kiwi system is characterized by the equation:
'
'	[ Vx ]		[  1	 	-1/2		-1/2	  ]	[ V1 ]
'	[ Vy ]	=	[  0		-sqrt(3)/2	sqrt(3)/2 ]	[ V2 ]
'	[  w ]		[  1		1		1	  ]	[ V3 ]
'
'	[1x3 matrx] = 	[3x3 matrix] * [1x3 matrix]
'
'where L is a length constant, Vx, Vy, and w are the system translation velocity components, and the system 
'angular velocity, and V1 is the wheel velocity vector at angle 0 degrees, V2 the wheel velocity vector at 
'angle -120 degrees, and V3 the wheel velocity vector at 120 degrees.

'
'Solving this system for V1, V2, and V3 yields the following equations of motion:
'
'	V1 = (2Vx	      + w) / 3
'	V2 = (-Vx - SQR(3)*Vy + w) / 3
'	V3 = (-Vx + SQR(3)*Vy + w) / 3
'
'(NOTE: These equations operate on the assumption that the magnitude of the desired velocity vector can be 
'no greater than 127. That is, the hypotenuse of the triangle formed by the trans_x and trans_y variable 
'cannot exceed 127. To ensure that this condition is met, one has to convert the rectangular 
'coordinates (trans_x, trans_y) into polar coordinates (V_desired, theta), limit V_desired to 127, then 
'back-calculate the new values of trans_x and trans_y based on the new value of V_desired and the already 
'existent theta. The code that does this has been placed exactly before this large block of text.)
'
'(NOTE: UPDATE! The Basic Stamp doesn't like the math required to make the previous note a reality, so I
'am using a crude solution: I scale down all input values by 127*SQR(2) to ensure that the largest vector 
'magnitude doesn't exceed 127.)
'
'The output from each equation (V1, V2, V3) is on the interval [-127, 127], but the desired outputs, 
'drill_1, drill_2, drill_3 are all on the interval [0, 254], so we compensate by setting drill_1, drill_2, 
'and drill_3 to the shifted version of V1, V2, and V3:
'
'	drill_1 = V1 + 127
'	drill_2 = V2 + 127
'	drill_3 = V3 + 127
'
'And that's it! Plug in all the variables, and we end up with the equations of motion that follow 
'(note: fractions were expanded out to compensate for the integer based calculation done by the stamp):

'Motion equations