====== How to Analyze a 4 Bar Linkage with LEGOs, Solidworks, MATLAB, Gyroscope, and Accelerometer ======
**Author:** <Xinke (Rebecca) Cao> 
\\
**Email:** <caox1@unlv.nevada.edu> 
\\
**Date:** Last modified on <07/27/2017>
\\
**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.
\\
\\
\\
\\
===== Building the LEGO 4-Bar Mechanism =====
{{:xinke:4b:q1.jpg?200 |}}
{{:xinke:4b:q2.jpg?200 |}}
{{:xinke:4b:q3.jpg?200 |}}
{{:xinke:4b:q4.jpg?200 |}}
{{:xinke:4b:q5.jpg?200 |}}
{{:xinke:4b:q6.jpg?200 |}}
{{:xinke:4b:q7.jpg?200 |}}
{{:xinke:4b:q8.jpg?200 |}}
{{:xinke:4b:q9.jpg?200 |}}
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\

===== Analyzing the 4-Bar Mechanism =====
See the picture below for a 4-bar mechanism:
\\
{{ :xinke:4b:4_bar.jpg?300 |}}
\\
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
  >> V*cos(rho+alpha)*alphaDOT - U*cos(sigma+gamma)*gammaDOT + W*betaDOT*cos(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
  >> V*cos(rho+alpha)*alphaDDOT - U*cos(sigma+gamma)*gammaDDOT - betaDOT^2 * W*sin(theta+beta) + betaDDOT*W*cos(theta+beta) + gammaDOT^2 * U*sin(sigma+gamma) - alphaDOT*2 * V*sin(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 =====
Please see [[4bMATLAB|this]] page.
\\
Please download the MATLAB model of the 4-bar mechanism [[https://drive.google.com/open?id=0B4prKwfGK8d2Z0FIN2Nwb3NjZ1U|here]].
\\
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.
\\
\\
===== Modeling in Solidworks =====
Please see [[4bSW|this]] page.
\\
Please download the Solidworks assembly [[https://drive.google.com/open?id=0B4prKwfGK8d2Nm56al8yc0Y0MTQ|here]].
\\
Please download the excel file that contains all of the Solidworks data [[https://drive.google.com/open?id=0B4prKwfGK8d2TTBmdElwMExRNHM|here]].
\\
\\
\\
<!-- ===== Accelerometer and Gyroscope for a Rotating Mechanism =====
Before I began the data collection with the accelerometer and the gyroscope, I tested it on a rotating mechanism, where the motor speed will cause the turntable to rotate with a constant speed. 
The gyroscope measures rotation, as explained in [[https://www.youtube.com/watch?v=rZxuwxOpLYU|this]] video.
The accelerometer measures the acceleration through a force measurement. 
The purpose of this part of the tutorial is for those reader who just want to get used to collecting data from the gyroscope and the accelerometer before testing it on a larger mechanism. 
\\
-->



<!-- ==== Sec 1: The Rotating Mechanism ====
Below are the materials needed for the construction of the rotating mechanism:
{{ :xinke:4b:materials.jpg?500 }}
  * one worm gear
  * 8 long friction pins
  * 6 half bushes
  * 12 bushes
  * 2 cross blocks
  * 4 double cross blocks
  * 4 11-straight beams
  * 4 right angled beams
  * 1 7-axle
  * 5 12-axle
  * 1 turntable gear
\\
Below are the pictured steps for constructing the rotating mechanism.
Get the two bushes and two 12-axles and two double cross blocks.
{{ :xinke:4b:1.jpg?300 }}
Insert the bushes, double cross blocks onto the turntable using an axle, like shown.
{{ :xinke:4b:2.jpg?300 }}
Gather two more bushes and double cross blocks.
{{ :xinke:4b:3.jpg?300 }}
Insert like so, then gather two 1/2 bushes and two full bushes.
{{ :xinke:4b:4.jpg?300 }}
Insert like so.
{{ :xinke:4b:5.jpg?300 }}
{{ :xinke:4b:6.jpg?300 }}
Flip the turntable over, gather 2 right-angle beams, 2 straight beams, and 4 long friction pins.
{{ :xinke:4b:7.jpg?300 }}
Insert the long friction pins on one of the straight beams like so.
{{ :xinke:4b:8.jpg?300 }}
Then, combine the right angle beams and the other straight beam. Insert a axle on the top slot of the turntable.
After you secure the axle that you've just inserted with a bush, gather two more 12-axles, two bushes, and two cross blocks.
Insert one bush and one cross block onto an axle.
{{ :xinke:4b:11.jpg?300 }}
Gather 2 right-angle beams, 2 straight beams, and 4 long friction pins.
{{ :xinke:4b:12.jpg?300 }}
Insert the long friction pins on one of the straight beams like so. 
In addition, as shown on the left side of the picture, insert the two 12-axles that was just assembled onto the turntable. Include the two straight beam & two right-angled beam assembly to the turntable.
{{ :xinke:4b:13.jpg?300 }}
Connect the other two straight beam & two right-angled beam assembly to the turn table.
{{ :xinke:4b:14.jpg?300 }}
Flip the turntable over, making sure that the two cross blocks stick out. Then gather 4 half-bushes and one full bush.
{{ :xinke:4b:15.jpg?300 }}
Secure the axles with the bushes that you just gathered. Then, gather the parts needed to connect the worm gear.
{{ :xinke:4b:16.jpg?300 }}
Insert the worm gear like so.
{{ :xinke:4b:17.jpg?300 }}

\\
-->


<!-- ==== Sec 2: Connecting the Sensors====
Below are the add-ons to the sensors that allows them to be attached to the rotating mechanism.
\\
{{:xinke:4b:connecting_sensors_1.jpg?140 |}}
{{:xinke:4b:connecting_sensors_2.jpg?200 |}}
{{:xinke:4b:connecting_sensors_3.jpg?200 |}} 
{{:xinke:4b:connecting_sensors_4.jpg?140 |}}
\\
\\
\\
\\
\\
\\
\\
\\
\\
\\
This is what the testing apparatus look like:
{{ :xinke:4b:fullsizerender.jpg?200 |}}
\\
\\
\\
-->
===== Coding in NXC to Read Sensor Data =====
Please download the [[https://drive.google.com/open?id=0B4prKwfGK8d2SkRGZ3JIMGZqUjA|code]].
\\
First, the mechanism with the sensors are placed on a flat surface, a function is run, as seen from [[https://www.youtube.com/watch?v=WCTdTN7qPOg|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);
  ReadSensorHTAccel(S1, tempX, tempY, tempZ);
  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 1: Connecting Sensors to the LEGO Bars ====
The hardware construction guide can be found on [[4bConstru|this]] page.
\\
\\


==== Sec 2: Realistic Motor Data ====
The results for measuring the motor speed can be found on [[4bmotor|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 ====
Please download the data and the filtering program for node "a1" [[https://drive.google.com/open?id=0B4prKwfGK8d2RURFSjNEOFNfQ1U|here]].
\\
Please download the data and the filtering program for node "b1" [[https://drive.google.com/open?id=0B4prKwfGK8d2VjJ5Tk95N3lnX1k|here]].
\\
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
  p_ad = polyfit(M(:,1),AngDisp,1);
   %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 [[4bdata|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. 
\\
\\