4bar

# How to Analyze a 4 Bar Linkage with LEGOs, Solidworks, MATLAB, Gyroscope, and Accelerometer

Author: <Xinke (Rebecca) Cao>
Keywords: LEGOs, 4 Bar, Solidworks, MATLAB, Gyroscope, Accelerometer

## Motivation and Audience

The 4-bar mechanism is commonly used in engineering. In addition, many textbooks covers it as well. This tutorial serves to allow people to

• be familiarized with the 4-bar mechanism
• derive the states of the 4-bar
• use MATLAB to model the 4-bar mechanism
• use Solidworks to model the 4-bar mechanism
• use the accelerometer and the gyroscope to obtain the states of the mechanism

Coding and modeling from the very beginning, this tutorial takes about a week to complete.

## Analyzing the 4-Bar Mechanism

See the picture below for a 4-bar mechanism:

Based on the drawing, the displacement vectors will add up to zero, which results in:

```>> W*cos(theta+beta)+V*cos(rho+alpha)-U*cos(sigma+gamma)-Gx = 0
>> W*sin(theta+beta)+V*sin(rho+alpha)-U*sin(sigma+gamma)-Gy = 0```

Taking the derivative of the displacement vectors will result in velocity vectors:

```>> -V*sin(rho+alpha)*alphaDOT + U*sin(sigma+gamma)*gammaDOT - W*betaDOT*sin(theta+beta)=0

Taking another derivative will result in acceleration vectors:

```>> -V*sin(rho+alpha)*alphaDDOT + U*sin(sigma+gamma)*gammaDDOT - betaDOT^2 * W*cos(theta+beta) - betaDDOT*W*sin(theta+beta) + gammaDOT^2 * U*cos(sigma+gamma) - alphaDOT*2 * V*cos(rho+alpha) = 0

With the displacement, velocity, and acceleration equations known (where there are 6 equations in total), you can solve for the unknowns (6 unknowns in total) using a program like MATLAB or solve numerically.

## MATLAB Program

This is divided so that the readers won't have to scroll up and down constantly due to the large amount of information on this page.

## Coding in NXC to Read Sensor Data

First, the mechanism with the sensors are placed on a flat surface, a function is run, as seen from this video that will calibrate the gyrosensor to read zero on the chosen surface.
To ask the NXT brick to collect data while running the motor, two different tasks are run at the same time, where the motor turning task is written as:

```task turnMotor(){
while (1) OnFwd(OUT_A,74);
} //end turnMotor```

Then, the main task will sample both the gyroscope and the accelerometer at a defined interval for a total of 10 seconds. Where, the data are collected at the end of each sampling period. The sampling period are 0.03 seconds.

```curTick = CurrentTick();

deltaTick = curTick - prevTick;
deltaTickInSeconds = deltaTick / 1000.0;
waitingTimeInSeconds = samplingTimeInSeconds - deltaTickInSeconds;
waitingTimeInMilliSeconds = waitingTimeInSeconds * 1000;
Wait(waitingTimeInMilliSeconds);

//wait until the end of sampling time to measure AND record data
angVelVal[i] = (SensorRaw(GYRO_PORT) * SCALE * WEIGHT - gyroZero)/ (SCALE * WEIGHT);
elapsedTimeInSeconds = elapsedTimeInSeconds + deltaTickInSeconds + waitingTimeInSeconds;
strElapsedTimeInSeconds = FormatNum("%5.3f", elapsedTimeInSeconds);```

After everything is done, the program will notify the user by playing a sound with a written message on the LCD screen.

`PlayTone(400,200); TextOut(0, LCD_LINE4, "Done!");`

## Accelerometer and Gyroscope for the 4 Bar Mechanism

### Sec 2: Realistic Motor Data

The results for measuring the motor speed can be found on this page.

### Sec 3: Data Processing in MATLAB

After the raw data has been collected by the NXT Brick, it is necessary to process these data in MATLAb.

Simple High Pass and Low Pass Filtering
Since at different stages, different frequencies of data need to be eliminated, I wrote a generic function named “Filter” that implements a simple high pass and low pass filtering.

```function [Low, High] = Filter(Original)
intLength = length(Original);
alpha = 0.35;
%initializing (by ensuring that Low and High are matrices
Low = Original;
High = Original - Low;
for i = 2:intLength
% start low filter
Low(i) = alpha*Original(i) + (1-alpha)*Low(i-1);

% start high filter
High(i) = Original(i) - Low(i);
end
end```

Integrated Data
When data are integrated, which uses the trapezoidal rule in MATLAB, the error of integration propagates as more data in the same array are being integrated. Therefore, knowing that the nature of the data is cyclic about the same center point, a linear regression can be fitted into the integrated data, then subtract this linear regression from the integrated data to filter out the errors of integration.

Taking the Derivative of Data
To take the derivative of the data, the centered difference method is used, as shown in the code below:

```function dydx = FirstDerivOf(x,y)
h = x(2)-x(1);
dy=diff(y);
% First Derivative Centered FD using diff
n=length(y);
for i=1:n-2
dydx(i)=(dy(i+1)+dy(i))/(2*h);  %corresponds to x(i)
end
end```

Combining Accelerometer and Gyroscope Data
Since the accelerometer data is more prone to noise, the gyroscope data is more prone to drift, and that tangential & centripetal accelerations can be derived from angular data, therefore, it is possible to implement a complementary filter to obtain a more accurate tangential and centripetal acceleration data.

```function D = ComplementaryFilter(OriAcc,OriGyro,alpha)
D = zeros(length(OriGyro),1);
for i = 1:length(OriGyro)
D(i) = OriGyro(i) * alpha + OriAcc(i) * (1-alpha);
end
end```

Converting to Horizontal Units Of course, we want everything to be in LEGO units, therefore, the following function is written to convert raw acceleration values into horizontal LEGO units.

```function [H] = toH_per_SecSquared(Original)
g = 9.81; %m/s^2
m2h = 1/8;
H = Original;
H = Original/200*g;  %this is in m/s^2
H = H*1000*m2h; %this is in h/s^2
end```

### Sec 4: MATLAB Program

First, I need to read data from a .csv file and store it in MATLAB.

```M = csvread('Acc.csv',1,0);
%1 column of M will be time
%2 column of M will be accelerometer x position
%3 column of M will be accelerometer y position
%4 column of M will be accelerometer z position
%5 column of M will be gyroscope angular velocity (deg/s)```

Since there is an offset between the center of the accelerometer and the nodes, an offset parameter is implemented to either amplify or diminish the accelerometer data.

```ac_c = 2; %the distance between accelerometer and center of rotation
act_c = 6; r = act_c;  %the actual distance between the node and the center of rotation
r_rat = act_c/ac_c;   %the ratio that the acc data need to be multiplied by```

To process the accelerometer data, the data are converted into horizontal units per second squared, then, the offset caused by gravity will be filtered out using a high pass filter. Next, noises from the accelerometer data will be filtered out using a low pass filter. Finally, the data are smoothed out.

```%%%%%%%%%% filtering accelerometer data
%convert to H/s^2 from raw data
M(:,2) = toH_per_SecSquared(M(:,2));
M(:,3) = toH_per_SecSquared(M(:,3));
%filter out the offset caused by gravity offset in accelerometer using high pass
[blank_,X] = Filter(M(:,2));
[blank_,Y] = Filter(M(:,3));
%filter out the noise from the accelerometer itself
[X,blank_] = Filter(X);
[Y,blank_] = Filter(Y);
%smooth out data
X = smoothdata(X); %in h/s^2
Y = smoothdata(Y);```

To process the gyroscope data, a low pass filter will be implemented to eliminate noise. Then, the data will be smoothed out. Since the drift is not significant within 10 seconds of sampling time, the drifting of the gyroscope will be ignored.

```%%%%%%%%%% filtering gyroscope data %%%%%%%%
%eliminate sudden spikes in the data by passing the data through a
%low-pass filter
[LowAngVel,blank_] = Filter(M(:,5));
%low values should be the values of angular velocity
%then, smooth the data out
LowAngVel = smoothdata(LowAngVel);
%find angular displacement
AngDisp = cumtrapz(M(:,1),LowAngVel);   %degrees
%filter out the integration error accumulated using linear regression
%fprintf('Slope for angular displacement is %f\n',p_ad(1));
%fprintf('Offset for angular displacement is %f\n',p_ad(2));
AngDisp = AngDisp - M(:,1)*p_ad(1); %subtract the linear regression```

To obtain the centripetal acceleration from gyroscope data, a little bit a physics is involved, where, in this case, centripetal acceleration is:

`(angular velocity)^2 * r`

thus,

```w = LowAngVel;  %in rad/s
a_n = w.^2.*r;```

To obtain the tangential acceleration from gyroscope data, again, we use physics, where, in this case, the tangential acceleration is:

`(angular accleration) * r`

thus,

`a_t = AngAcc .* r;`

The tangential velocity can also be obtained from the gyroscope data, where:

`TanVelocity_g = w*r;`

After having data from gyroscope and accelerometer, both sets of data will be fed into the complementary filter, where the output data from the complementary filter will be used to find the x and y components of velocity and acceleration.

```x_acc = a_n_compl(2:end-1).*cos(AngDisp(2:end-1))+a_t_compl.*cos(AngDisp(2:end-1)+pi/2);
y_acc = a_n_compl(2:end-1).*sin(AngDisp(2:end-1))+a_t_compl.*sin(AngDisp(2:end-1)+pi/2);
x_vel = TanVelocity.*cos(AngDisp(2:end-1)+pi/2);
y_vel = TanVelocity.*sin(AngDisp(2:end-1)+pi/2);```

### Sec 5: Filtered Data

The resultant filtered data from MATLAB can be found on this page.

## Final Words

This tutorial's objective was to familiarize the user with the 4 bar mechanism and two sensors from HiTechnic (accelerometer and gyroscope). Once the concepts were conveyed the reader could use the accelerometer and gyroscope to measure the states of other mechanisms.

Speculating future work derived from this tutorial, includes being able to use filters on the data obtained from gyroscope and accelerometer. In addition, the readers can improve on the filters discussed in this tutorial.