Why is My 4 Bar Linkage Animation Code Causing the Coupler Link to Stretch?

[bnshrdr]

I know there are programs out there that can do this already, but I want to keep sharp on my programming skills.

All I want to do is animate a grashof 4 bar mechanism. The program I wrote does this correctly for the most part. The problem is that when the crank gets around to around -45 degrees with the horizontal, my coupler link stretches for maybe about 20 degrees and then it goes and rotates how it should.

I have been pounding my head and just can't figure out where the problem in the code lies. I know I am asking a lot because my program is using formulas I derived on paper and they look rather sloppy. However it is commented and if anyone is willing to take a look I would appreciate it.

Here is a video of the mechanism:


Here is the code:

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <plot.h>

int plotdata (double (*plot)[3][4], char (*fname)[10])
{
    plPlotter *plotter;
    plPlotterParams *plotter_params;
    FILE *fp;
    int buf = 1000;
      
    plotter_params = pl_newplparams ();
    pl_setplparam (plotter_params, "PAGESIZE", "letter");
    
    fp = fopen(*(fname), "wb");
    
    if ((plotter = pl_newpl_r ("png", stdin, fp, stderr, plotter_params)) == NULL)
    {
        fprintf (stderr, "Couldn't create Plotter\n");
        return 1;
    }
      
    if (pl_openpl_r (plotter) < 0)
    {
        fprintf (stderr, "Couldn't open Plotter\n");
        return 1;
    }
    
    pl_fspace_r (plotter, 0.0, 0.0, 3000.0, 3000.0);
    pl_flinewidth_r (plotter, 0.25);
    pl_pencolorname_r (plotter, "red");
    pl_erase_r (plotter);

    pl_line_r(plotter, buf, buf, (*plot)[0][2] + buf, (*plot)[0][3] + buf);
    pl_line_r(plotter, (*plot)[0][2] + buf, (*plot)[0][3] + buf, (*plot)[1][2] + buf, (*plot)[1][3] + buf);
    pl_line_r(plotter, (*plot)[1][2] + buf, (*plot)[1][3] + buf, (*plot)[2][2] + buf, (*plot)[2][3] + buf);
        
    if (pl_closepl_r (plotter) < 0) 
    {
        fprintf (stderr, "Couldn't close Plotter\n");
        return 1;
    }
      
    if (pl_deletepl_r (plotter) < 0)  
    {
        fprintf (stderr, "Couldn't delete Plotter\n");
        return 1;
    }
    fclose(fp);
    
    return 0;
}

int main (void)
{
    double L0, L1, L2, L3, Ld, theta, theta2, phi;
    double plot[3][4];
    char fname[10];
    int i;
    L0 = 700;
    L1 = 300;
    L2 = 800;
    L3 = 600;
    
    /* theta = angle between ground and driver */
    theta = M_PI;
    for (i = 0; i < 72; i++)
    {
        /* decrease theta by 5 degrees each cycle */
        theta = ((theta * 180 / M_PI) - 5) * M_PI / 180;
        
        /* Ld = distance from end of driver to end of follower */
        Ld = sqrt(pow(L0, 2) + pow(L1, 2) - (2*L0*L1*cos(theta)));
        
        /* phi = angle between coupler and follower */
        phi = acos((pow(L2, 2) + pow(L3, 2) - pow(Ld, 2)) / (2*L2*L3));
        
        /* calculate angle between follower and ground outside of mechanism */
	theta2 = M_PI - asin(L1/Ld*sin(theta)) - asin(L2/Ld*sin(phi));
            
        /* coordinates for driver (x1,y1,x2,y2) */
        plot[0][0] = 0;
        plot[0][1] = 0;
        plot[0][2] = L1*cos(theta);
        plot[0][3] = L1*sin(theta);
                
        /* coordinates for coupler (x1,y1,x2,y2) */
        plot[1][0] = plot[0][2];
        plot[1][1] = plot[0][3];
        plot[1][2] = L0 + (L3*cos(theta2));
        plot[1][3] = (L3*sin(theta2));
            

        /* coordinates for follower (x1,y1,x2,y2) */
        plot[2][0] = plot[1][2];
        plot[2][1] = plot[1][3];
        plot[2][2] = L0;
        plot[2][3] = 0;
        
        /* format the file name for plot */
        itoa(i, fname, 10);
        if (i > 9)
            strcpy(fname+2, ".png");
        else
            strcpy(fname+1, ".png");

        /* plot the data into a file */
        plotdata(&plot, &fname);
    }

    return 0;
}

PS: I use the gnu libplot to do my plotting for me. Was pretty simple for me to figure out.

 

[Mark44]

I know there are programs out there that can do this already, but I want to keep sharp on my programming skills.

All I want to do is animate a grashof 4 bar mechanism. The program I wrote does this correctly for the most part. The problem is that when the crank gets around to around -45 degrees with the horizontal, my coupler link stretches for maybe about 20 degrees and then it goes and rotates how it should.

I have been pounding my head and just can't figure out where the problem in the code lies. I know I am asking a lot because my program is using formulas I derived on paper and they look rather sloppy. However it is commented and if anyone is willing to take a look I would appreciate it.

Here is a video of the mechanism:


Here is the code:

#include <stdio.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <plot.h>

int plotdata (double (*plot)[3][4], char (*fname)[10])
{
    plPlotter *plotter;
    plPlotterParams *plotter_params;
    FILE *fp;
    int buf = 1000;
      
    plotter_params = pl_newplparams ();
    pl_setplparam (plotter_params, "PAGESIZE", "letter");
    
    fp = fopen(*(fname), "wb");
    
    if ((plotter = pl_newpl_r ("png", stdin, fp, stderr, plotter_params)) == NULL)
    {
        fprintf (stderr, "Couldn't create Plotter\n");
        return 1;
    }
      
    if (pl_openpl_r (plotter) < 0)
    {
        fprintf (stderr, "Couldn't open Plotter\n");
        return 1;
    }
    
    pl_fspace_r (plotter, 0.0, 0.0, 3000.0, 3000.0);
    pl_flinewidth_r (plotter, 0.25);
    pl_pencolorname_r (plotter, "red");
    pl_erase_r (plotter);

    pl_line_r(plotter, buf, buf, (*plot)[0][2] + buf, (*plot)[0][3] + buf);
    pl_line_r(plotter, (*plot)[0][2] + buf, (*plot)[0][3] + buf, (*plot)[1][2] + buf, (*plot)[1][3] + buf);
    pl_line_r(plotter, (*plot)[1][2] + buf, (*plot)[1][3] + buf, (*plot)[2][2] + buf, (*plot)[2][3] + buf);
        
    if (pl_closepl_r (plotter) < 0) 
    {
        fprintf (stderr, "Couldn't close Plotter\n");
        return 1;
    }
      
    if (pl_deletepl_r (plotter) < 0)  
    {
        fprintf (stderr, "Couldn't delete Plotter\n");
        return 1;
    }
    fclose(fp);
    
    return 0;
}

int main (void)
{
    double L0, L1, L2, L3, Ld, theta, theta2, phi;
    double plot[3][4];
    char fname[10];
    int i;
    L0 = 700;
    L1 = 300;
    L2 = 800;
    L3 = 600;
    
    /* theta = angle between ground and driver */
    theta = M_PI;
    for (i = 0; i < 72; i++)
    {
        /* decrease theta by 5 degrees each cycle */
        theta = ((theta * 180 / M_PI) - 5) * M_PI / 180;
        
        /* Ld = distance from end of driver to end of follower */
        Ld = sqrt(pow(L0, 2) + pow(L1, 2) - (2*L0*L1*cos(theta)));
        
        /* phi = angle between coupler and follower */
        phi = acos((pow(L2, 2) + pow(L3, 2) - pow(Ld, 2)) / (2*L2*L3));
        
        /* calculate angle between follower and ground outside of mechanism */
	theta2 = M_PI - asin(L1/Ld*sin(theta)) - asin(L2/Ld*sin(phi));
            
        /* coordinates for driver (x1,y1,x2,y2) */
        plot[0][0] = 0;
        plot[0][1] = 0;
        plot[0][2] = L1*cos(theta);
        plot[0][3] = L1*sin(theta);
                
        /* coordinates for coupler (x1,y1,x2,y2) */
        plot[1][0] = plot[0][2];
        plot[1][1] = plot[0][3];
        plot[1][2] = L0 + (L3*cos(theta2));
        plot[1][3] = (L3*sin(theta2));
            

        /* coordinates for follower (x1,y1,x2,y2) */
        plot[2][0] = plot[1][2];
        plot[2][1] = plot[1][3];
        plot[2][2] = L0;
        plot[2][3] = 0;
        
        /* format the file name for plot */
        itoa(i, fname, 10);
        if (i > 9)
            strcpy(fname+2, ".png");
        else
            strcpy(fname+1, ".png");

        /* plot the data into a file */
        plotdata(&plot, &fname);
    }

    return 0;
}

PS: I use the gnu libplot to do my plotting for me. Was pretty simple for me to figure out.

Cool video!
I see what you're saying about the coupler stretching when the angle gets to about -45 degrees. It's possible that you're running into some problems with the asin and/or acos functions when you calculate phi and theta2. acos returns an angle in the range [0..pi] (radians). asin returns an angle in the range [-pi/2..pi/2].

To debug this, I would do the following:
1. Verify that the formulas used are correct. The first calculation, for Ld, looks like it's using the Law of Cosines, and seems correct. I didn't check your calculation for phi and theta2, so you might want to double check them. Since the animation seems to be doing the right thing for most of the cycle, I doubt that the problem is in your formulas, although you might be getting incorrect numbers for some values of the angles returned by asin and acos, as already noted.

2. Figure out three or four values of theta that put the mechanism in the interval where you're noticing a problem. The values of theta should bracket -45 degrees. By hand, calculate the starting and ending points of the link that is stretching, and write them down.

Modify the loop in main so that it does the same calculations for the three or four values of theta, and have it print out the starting and ending points of that link. Compare the results from your program with the hand-calculated results, and that should give you an idea of what's going wrong.

Finally, and this is unrelated to the problem you're experienceing, you are using pow() to calculate the squares of several numbers. I would instead just multiply the numbers. E.g., instead of doing this:

Ld = sqrt(pow(L0, 2) + pow(L1, 2) - (2*L0*L1*cos(theta)));

I would do this:

Ld = sqrt(L0 * L0 + L1 * L1 - (2*L0*L1*cos(theta)));

It might not make a difference in plotting speed, but the calculations would happen faster.

 

[bnshrdr]

Aha! I have fixed it.
I had to change the theta2 calculation to the following:

theta2 = M_PI - asin(L1/Ld*sin(theta)) - (M_PI-phi-asin(L3*sin(phi)/Ld));

When I drew it out on paper, I had one sign wrong...thus is the life of an engineer.
Also I changed the way I calculated it slightly (less code, would have had to have done an if statement)

Here is a new video of it. It appears at least to me to function correctly. Does this look like a correct animation to anyone else?

 

[Mark44]

Looks good to me.