View Single Post
  #13   Spotlight this post!  
Unread 24-05-2015, 21:56
Jared's Avatar
Jared Jared is offline
Registered User
no team
Team Role: Programmer
 
Join Date: Aug 2013
Rookie Year: 2012
Location: Connecticut
Posts: 602
Jared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond reputeJared has a reputation beyond repute
Re: Kiwi Field Oriented Driving Programming

I also don't know which direction the gyro will consider to be a positive angle, so if the following doesn't work, try putting a negative sign in front of the gyro angle. I'm assuming that a counterclockwise rotation as viewed from above will be considered a positive angle because that's how the gyro on my desk in front of me works.

The easiest way to accomplish this would be to reuse the code you already have that calculates your wheel speeds when you give it x, y, and rotation. Currently, you have the joystick axes mapped directly to the x, y, and rotation inputs on the kiwi drive code. To achieve field centric control, you need to modify the x and y values. The rotation part doesn't need to be changed. j_x and j_y will represent the x and y values from the joystick, and theta will be the angle reported from the gyroscope, with an angle of zero representing the robot facing forward. x and y represent the values you pass to your code that calculates the wheel speeds that you already have.

x = j_x * cos(theta) + j_y*sin(theta)
y = -j_x * sin(theta) + j_j * cos(theta)


The whole thing put together should look like this. Again, j_x, j_y, and j_z all represent joystick values, and wheel 1, wheel 2, and wheel 3 are arranged like in Ether's Kiwi Omniwheel Inverse Kinematics paper you linked.
Code:
x = j_x * cos(theta) + j_y*sin(theta)
y = -j_x * sin(theta) + j_j * cos(theta)
z = j_z
wheel1 = x + z
wheel2 = -x/2 + 0.866y + z
wheel3 = -x/2 - 0.866y +z
You can figure out where these equations come from if you draw triangles between your desired direction of travel, your field coordinate system, and your robot coordinate system.