Lab 5: Make Something Engaging

Lab 5: Make Something Engaging

PID implementation and Haptic + graphic experience.

Q1-Modify the Arduino source control to accomplish the same output without using the provided library PID controller.

There are several ways to calculate and implement PID controller on microprocessors. In previous post we explained the difference between sampling methods and importance of sampling time in designing a controller. While it is feasible to tune the controller parameters after defining the sampling rate, it requires many trial and error attempts and will not be very accurate in practice. therefore, we need to have a mapping system to transfer our PID controller from continuous time into discrete one. To do that we need to transfer our system model from S plane to Z plane. From several different methods of transformation we will explain backward difference approximation (B.D) and Tustin model. Our PID controller can be written as:

(1)

We also consider a filter for our derivative term. By defining N we can change the position of pole of derivative term.

   (2)

to map from S domain to Z domain we can use the following approximation in which h is the sampling time.

 (3)

by substituting (3) to (1) we will have the general equation as follow: 

(4)

for different method we can find the a and b coefficients according to following table.

 and in order to implement that in microprocessor we just need to code this line:

Arduino PID Code:

To make our digital controller, first we need to make a fixed sampling time. I just used timer interrupt to read the encoder value at every 1 ms.

#include "Twiddlerino.h"
#include "PID_v1.h"
#include <MsTimer2.h>

void setup()
{
  Serial.begin(115200);
  TwiddlerinoInit();
  target = 0;
    MsTimer2::set(h,read);
    MsTimer2::start();

}

then in the read function, I made my controller  as follow:

void read(){
                 K=p;
                 Ti=i;
                 Td=d;
                 Tf=Td/N;
                 H=h/1000;
               //  a0=2*Ti*(2*Tf+H);// Tustin begins
               //  a1=-8*Ti*Tf;
               //  a2=2*Ti*(2*Tf-H);     
               //  b0=K*(H*H+2*(Ti+Tf)*H+4*Ti*(Td+Tf));
               //  b1=K*(2*H*H-8*(Td+Tf)*H-2*H*(Ti+Tf));
               //  b2=K*(H*H+4*Ti*(Ti+Tf)+2*H*Ti*(Ti+Tf)); //Tustin ends
                 b0=(K+(Ti*H*0.5)+((2*Td)/H));// Bilinear approximation begins
                 b1=((Ti*H)-(4*(Td/H)));
                 b2=-K+(Ti*H*0.5)+((2*Td)/H);
                 a2=1;
                 a1=0;
                 a0=1; // bilinear approximation ends
                     pos2=pos1;
                     pos1=pos;
                    pos=ReadEncoder();
                     er0=pos-target;
                     er1=pos1-target;
                     er2=pos2-target;
                     //Uout=(b0*er0+b1*er1+b2*er2+a1*Uout1+a2*Uout2)/a0;
                     Uout2=Uout1;
                     Uout1=Uout; 
                     Uout=K*er0+i*er0*H+d*(er1-er2)/H; //simple error and integral terms
          }

Q2- Make an engaging experience!

In this part, I decided to use a 3D Puma Robotic Demo library from Matlab and connect  it to the Twiddler.  This code presents a 6 degree of freedom robotic arm, which can simulate forward and reverse kinematics.The Idea is that user can select and control any degree of freedom that they want. Based on the size of the motor, the PID coefficients will change so that for example by moving theta1 which is a large motor, they feel, large control effort while for moving the robot End-effector (small motor) they can rotate it much more easily.

Fig 1, 3D puma robotic arm (@Mathworks)

Implementation:

First, I wrote the codes that can map the slidebar position to the target point of Twiddler so by moving the slide bar we can feel the movement and visually see it. However the communication was slow and there was a delay between visual command and physical response. Second I made a push button  called TControl to connect the position of the Twiddler to robotic arm. When being pressed, user can move the arm with Twiddler and feel the motor reaction which tries to bring it back to set position. As we can see in Vid 1, By moving the slide bar we can change the set point. To have more smooth tracking of the handler I changed the time interval for sending data from arduino to matlab. 

Next step:

To solve the problem of delay we need to empty the of arduino's buffer  before trying to write on serial port or we can send a package of data or sending data by software's requests. As we can see in Vid 1, by pressing the TControl, we see first  the memory of previous movements is running and then it starts working. I didn't write the codes to select different degrees of freedom and change the PID  coefficients based on them, and it would be further steps of this projec.

Vid1. Controlling robot arm with haptic feedback