1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
|
* Function: Calculate the joint angles needed to produce the circle
*
*********************************************************************************************
*
* Variables | function | units
*
|
* theta_1 | joint angle 1 | radians
* theta_2 | joint angle 2 | radians
* x | x location | inches
* y | y location | inches
* num_points| number of points to convert|
* t | time | seconds
* i | loop variable |
*********************************************************************************************/
/* Header Files */
#include <stdio.h>
#include <math.h>
#define A1 20.0
#define A2 10.0
#define X_C 20.0
#define Y_C 0.0
#define PI 3.14159265
/*prototypes*/
double r, t;
double get_x();
double get_y();
double get_theta2( double x, double y);
double get_theta1( double x, double y, double theta_2);
void q_for_quit();
/*Start of main function*/
int main(void)
{
/* declarations: */
int num_points, i;
printf(" PLASMA CUTTER\n");
printf("------------------------------------------------------------\n\n");
printf("Please enter the radius of the circle [in]: \n");
scanf("%f", &r);
if( r < 5 && r > 10)
printf("Please enter a valid number for the radius
printf("Number of points needed on the circle: \n");
scanf("%d", &num_points);
printf("The data file contains %d records.\n", num_points);
printf(" RESULTS\n");
printf("------------------------------------------------------------\n\n");
printf(" LINK LENGTH 1[in]: %5.1f\n", A1);
printf(" LINK LENGTH 2[in]: %5.1f\n", A2);
/*Print the results*/
printf("\n");
printf(" CIRCLE CUTTING PROGRAM\n");
printf(" time[sec] theta_1[rad] theta_2[rad]\n");
printf("%lf", &theta1);
/*Calculate the time step*/
for( i = 0; i< num_points; i++)
}
void q_for_quit()
{
char g;
/*This keeps the window open*/
printf("\n\nPress enter to q to quit.\n");
do{
g = getchar();
}while( g != 'q' );
return;
}
/*Function to find x*/
double get_x()
{
double x;
x = r*cos(t*PI)+ X_C;
return x;
}
/*Function to find y*/
double get_y()
{
double y;
y = r*sin(t*PI)+ Y_C;
return y;
}
/*Function to calculate the intermediate value D*/
double get_D( double x, double y)
{
double D;
D = (x*x + y*y -A2*A2 - A1*A1)/(2.0 * A1 * A2);
return D;
}
/*Based on the given values of x and y calculate the angle of joint 2*/
double get_theta2(double x, double y)
{
double D;
D = get_D(x, y);
return atan2(sqrt(1-D*D), D);
}
/*given the values of x, y, and z calculate and return the value of theta 2*/
double get_theta1( double x, double y, double theta_2)
{
return atan2(y,x) - atan2(A2*sin(theta_2), A1+A2*cos(theta_2));
}
|