/* A dual iterative program in CH for RCCC mechanism in paper "Dual Iterative Displacement Analysis of Spatial Mechanisms Using the \CH\ Programming Language," Mechanism and Machine Theory, by Cheng, H. H., and Thompson, S. */ #include /* PI is defined in */ #define D2R PI/180 #define R2D 180/PI main(){ int i, j, theta1, n=4, m=6; float error, epsilon=0.00001, MaxError=100000; dual theta[n], alpha[n]; array dual Q[3][3],Bi[3][3],B1[3][3],Ai[3][3],M[m][3],d[3],v[m],N[3][3],u[3]; void ai(dual theta, alpha, array dual Ai[3][3]){ /* calculates Ai matrix */ Ai[0][0] = cos(theta); Ai[0][1] = -sin(theta)*cos(alpha); Ai[0][2] = sin(theta)*sin(alpha); Ai[1][0] = sin(theta); Ai[1][1] = cos(theta)*cos(alpha); Ai[1][2] = -cos(theta)*sin(alpha); Ai[2][0] = 0; Ai[2][1] = sin(alpha); Ai[2][2] = cos(alpha); /*(1) */ } alpha[0]=dual(30*D2R,2); alpha[1]=dual(55*D2R,4); alpha[2]=dual(45*D2R,3); alpha[3]=dual(60*D2R,5); /* link parameters */ theta[0]=dual(0,0); theta[1]=dual(100*D2R,0); theta[2]=dual(100*D2R,0); theta[3]=dual(100*D2R,0); /* initial values */ printf(" theta1 s1 theta2 s2 " "theta3 s3 theta4 s4\n"); /* print heading */ Q = 0; Q[0][1] = -1; Q[1][0] = 1.0; /*(9) */ for(theta1=0;theta1<=360;theta1+=90){ /* 90 deg step for theta1 */ theta[0] = theta1*D2R; /* change deg to rad */ do{ for (i=1;i= epsilon && error < MaxError); if(error >= MaxError) /* diverged */ printf("Error: the iterative algorithm diverges at theta1 = %d \n", theta1); else /* converged, print results */ printf("%8.2f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f\n", real(theta[0])*R2D,dual(theta[0]),real(theta[1])*R2D,dual(theta[1]), real(theta[2])*R2D,dual(theta[2]),real(theta[3])*R2D,dual(theta[3])); } } /**** The output from execution of the above CH program theta1 s1 theta2 s2 theta3 s3 theta4 s4 0.00 0.000 149.679 -0.210 45.556 -2.693 144.209 -0.115 90.00 0.000 54.512 -3.171 92.715 -1.513 81.114 -2.114 180.00 0.000 -59.094 -0.301 142.648 -1.814 83.700 -0.173 270.00 0.000 -157.692 1.136 92.714 -1.513 148.494 -0.515 360.00 0.000 -210.320 -0.210 45.556 -2.693 144.209 -0.115 ****/