Pages2

Monday, April 27, 2015

Integrating Circular Motion Estimates in 2D



1. Theory


2. Example
Circular motion start at (Px1,Py1,theta1) = (5,4,128.6598o) with 30o angular displacement.



3. Code

#include<iostream>
#include <iomanip>
#include <math.h>
#define PI 3.14159265
using namespace std;

int main ()
{     
       double motion0[4] = {5,4,128.6598,30};// Example Starting :{Px1,Py1,theta1,dtime}
       double dtheta = 30 * (PI/180);
       double radius = 6.4031;
       double motion1[4] ; // {Px2,Py2,theta2,dtime}
      
       cout << motion0[0] << ";" << motion0[1] << ";" << motion0[2] << endl;
      
motion0[2] = motion0[2] * (PI/180); //Change to radians
for (int i = 0; i < 20; i++)
       {            
motion1[0] = motion0[0] + radius *( sin(dtheta+motion0[2]) - sin(motion0[2]) ); // Equation of Xnew      
motion1[1] = motion0[1] - radius *( cos(dtheta+motion0[2]) - cos(motion0[2]) ); // Equation of Ynew
              motion1[2] = motion0[2] + dtheta; // Equation of thetanew
             
cout << motion1[0] << ";" << motion1[1] << ";" << motion1[2]*(180/PI) << endl;  

              motion0[0] = motion1[0]; motion0[1] = motion1[1];motion0[2]=motion1[2];   
              //Replace Px1,Py1 and theta1 with Xnew, Ynew and thetanew
                           
}     

       system("PAUSE");
       return 0;

}

4. Result


No comments:

Post a Comment