OCCRA [more] FRC Kickoff : 22d 06h 29m ...
Adult coaches are the "rock stars" of our program, and I know on Simbotics the kids really do look up to and aspire to be a great as these coaches. - Karthik [more]
 Chief Delphi Position Control
 User Name Remember Me? Password
 CD-Events CD-Media CD-Spy FRC-Spy
The Chief Delphi Forums are sponsored by Innovation First International, Inc.

#1
03-04-2010, 09:40 PM
 biojae Likes Omni drives :) AKA: Justin Stocking FTC #5011 (BOT SQUAD) && FTC#72(Garage bots)&& FRC#0399 (Eagle Robotics) Team Role: College Student Join Date: Oct 2008 Rookie Year: 2008 Location: Lancaster Posts: 276
Position Control

Over the fall, I made a simple positioning system using simple Trig and geometry.
That was using the PWM open loop (On the Jaguar at least) voltage control. Now that we are allowed to use CAN, I would like to use the Position control mode. The encoders are hooked directly into the Jaguar.

The simple formulas that I used are as follows:
Code:
```S = ((Delta Dist Rightside) + (Delta Dist Leftside)) / 2.0;
X += S * cos(theta);
Y += S * sin(theta);```
Theta is from a 300 deg/sec yaw rate gyro.

So, once I had a new estimated position, the distance and angle to the next point were calculated.
The angle and distance would then go into 2 PID calculations.
Each side of the drive would then be set from a simple arcade drive function

Code:
```double angle = MathUtils.atan2(pY-Y, pX-X);
double distance = Math.sqrt(Math.pow(pY-Y,2)+Math.pow(pX-X,2));

double leftSpeed  = speed + turn;
double rightSpeed = speed - turn;```
That worked nicely, but now I would like to have the Jaguar handle some of the calcs and PID itself.

I would like to use the Jaguar's Position control, tell it how far to turn the wheel and it goes there.

One dimensional movement (forward / backward) is easy, as the distance to tell it is from the distance formula, but once it gets to rotation, I am lost.

From the above equations, I derived these equations:

Code:
```Dist Left = sqrt((y-y1)^2 + (x-x1)^2) + (atan2(y-y1,x-x1) - theta1)
Dist Right = sqrt((y-y1)^2 + (x-x1)^2) - (atan2(y-y1,x-x1) - theta1)```
would this work if the distance from each side were to be scaled to rotations? or did I make a mistake with my math?
__________________
FTC Team 72 - No site
FRC Team 399 - http://www.team399.org
2010 Rockwell Collins Innovation in Control Award - (Use of the CAN bus, among other reasons) Phoenix, Arizona!

Last edited by biojae : 03-04-2010 at 10:05 PM.
#2
03-05-2010, 07:42 PM
 AmoryG Registered User FRC #2423 (KwarQs) Team Role: Alumni Join Date: Mar 2008 Rookie Year: 2008 Location: Watertown, MA Posts: 192
Re: Position Control

I don't know how to fully answer your question, but I'll do it the best I can. I'll assume your robot is like ours (I know it probably isn't) and that these equations will work just as well with your robot as it did with ours.

Assuming that all wheels on a single side of your robot travels at the same rate, let's look at my horrible paint job picture:

Let R equal the speed of the right wheels (using an encoder on your right wheel),

et L equal the speed of the left wheels (using an encoder on your left wheel),

and let Base equal the width of your robot, or the distance between two wheels.

As you may know, to get angular speed, you can use the equation:

Angular Speed (rad/sec) = velocity (m/sec) / radius (m)

Since we're not recording speed, but angle and position, ignore velocity and use distance for now.

Since R is larger than L, relative to L, the right side of the robot has moved R - L. Let velocity (or distance) equal R - L and make Base the radius.

From that you get:

theta = (R - L) / Base, or the amount your robot has turned.

To get the heading in the x and y direction, you will need to know the point your robot is driving around. The distance traveled is greater on the right side, so your robot will arc (or drive) to the left. That means the point your robot is driving around will be somewhere to the left of your robot:

The radius of the arc can be calculated by finding the distance between the center of the circle and the center of your robot, or the length of the triangle - half the base.

To calculate the length of the triangle take the ratio between Base and your robot's relative speed (R - L). Multiply that ratio by the height of your triangle (or R) and you get your length:

R * Base / (R - L).

Subtract half the base from that and you get

R * Base / (R - L) - Base / 2
which equals:
Base ( R / ( R - L ) - 1 / 2)
which when R / (R - L) and 1 / 2 put into one fraction simplfies to:
Base ( (2R - R + L) / 2( R - L))
which equals:
Base ( (R + L) / 2(R - L))

Since theta equals (R - L) / Base the equation can then be simplified to:

(R + L) / (2 * theta)

Radius = (R + L) / (2 * theta)

Using your Radius and theta, you can get x and y.

y = Radius * sin(theta)
x = Radius * (1 - cos(theta))

1 - cos(theta) because your actual x is radius minus x

Of course these are all relative values, so you will need to recalculate based on the facing of your robot.

Hope that helps!
__________________
KwarQs 2423

2008 Boston Regional Rookie Allstars

http://whsrobot.blogspot.com/
#3
03-05-2010, 10:11 PM
 biojae Likes Omni drives :) AKA: Justin Stocking FTC #5011 (BOT SQUAD) && FTC#72(Garage bots)&& FRC#0399 (Eagle Robotics) Team Role: College Student Join Date: Oct 2008 Rookie Year: 2008 Location: Lancaster Posts: 276
Re: Position Control

Thank you.

I have this working when I use PWM, i set the speed of each wheel, then every loop of the periodic, i calculate the new position based off of the distance traveled.
That part works.

What I would like to do now, is to use the jaguar's position control mode
(you tell it how far to rotate, and it rotates that far), and tell each side exactly how far to go instead of a % voltage.

Using the distance formula for one d.o.f. movement is easy, it calculates the distance between both points, and it travels that far.

The problem is when rotation is involved, with the % voltage I could give precedence to turning because i had two pid equations feeding the outputs to the motors, but with position control, more precedence is given to driving the distance, is there anyway to fix that?
__________________
FTC Team 72 - No site
FRC Team 399 - http://www.team399.org
2010 Rockwell Collins Innovation in Control Award - (Use of the CAN bus, among other reasons) Phoenix, Arizona!

 Thread Tools Display Modes Rate This Thread Linear Mode Rate This Thread: 5 : Excellent 4 : Good 3 : Average 2 : Bad 1 : Terrible

 Posting Rules You may not post new threads You may not post replies You may not post attachments You may not edit your posts vB code is On Smilies are On [IMG] code is On HTML code is Off
 Forum Jump User Control Panel Private Messages Subscriptions Who's Online Search Forums Forums Home Announcements     User Announcements FIRST     General Forum         FIRST E-Mail Blast Archive     Rumor Mill     Career     Robot Showcase     Moderated Discussion Technical     Technical Discussion     Robotics Education and Curriculum     Motors     Electrical         CAN     Programming         NI LabVIEW         WindRiver C++         Java         MATLAB & Simulink         Python     intelitek easyC/REC information         easyC v2 for Vex         easyC for FRC         REC/Learnmate         WPILib         easyC PRO     Control System         IFI Control System         FRC Control System         Sensors         USB Chicklet     Pneumatics     Kit & Additional Hardware     CAD         Inventor         SolidWorks         Pro ENGINEER / Creo     IT / Communications         3D Animation and Competition         Website Design/Showcase         Videography and Photography         Computer Graphics     National Instruments LabVIEW and Data Acquisition         LabView and Data Acquisition Competition     Awards         Chairman's Award     Rules/Strategy         Scouting         You Make The Call     Team Organization         Fundraising         Starting New Teams         Finding A Team         College Teams     Championship Event     Regional Competitions     FIRST in Michigan     Mid-Atlantic Robotics     Off-Season Events         Chief Delphi Invitational     Thanks and/or Congrats     FRC Game Design     OCCRA         OCCRA Q&A         OCCRA Programming Other     Unsung FIRST Heroes     Chit-Chat         Games/Trivia             Fantasy FIRST     Car Nack's Corner     College & University Education     Dean Kamen's Inventions     FIRST-related Organizations         Western Region Robotics Forum         Robot Chicks Union (RCU)         Southern California Regional Robotics Forum         FIRST Interactive Rural SupporT         FIRST Scouting Network         The Grand FIRST Team         FloridaFIRST         FIRSTwiki         Ultimate Robot Challenge         Team San Diego         Dream FIRST         The Blue Alliance             Video Archives     FIRST Historians     FIRST-A-Holic's Anonymous Mailbox     FIRST In the News...     FIRST Lego League         Lego Mindstorm Discussion     FIRST Tech Challenge     VEX         VEX Robotics Competition         VEX IQ     Televised Robotics     Math and Science         NASA Discussion ChiefDelphi.com Website     CD Forum Support     Extra Discussion Old Forum Archives     1999     2000     2001

 Similar Threads Thread Thread Starter Forum Replies Last Post CAN Jaguar Enable Control .vi Initial Encoder Position TonyP CAN 0 02-26-2010 07:23 PM CAN Position control code? taichichuan WindRiver C++ 8 02-13-2010 11:46 AM Position Control using potentiometer? biojae CAN 13 02-03-2010 08:12 PM Antenna position Torboticsmember Rules/Strategy 2 02-16-2008 11:18 AM Which position is A vs. C MattB703 Rules/Strategy 3 03-29-2006 08:56 AM

All times are GMT -5. The time now is 05:30 PM.

The Chief Delphi Forums are sponsored by Innovation First International, Inc.

 -- English (12 hour) -- English (24 hour) Contact Us - Chief Delphi - Rules - Archive - Top