DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
** #WEEK 1 REPORT **
 
 {{::week1_ballandbeam.jpg?200|}}
 
 **# Finished building the structure.**
 
 **# Sensor limitations:**      
 I got the Syntax to get the reading of this sensor in the seller's website (I didn’t find a function to retrieve the value, like the US sensor (just put SensorUS(port) and you get the sensor reading)[[http://www.mindsensors.com/ev3-and-nxt/59-high-precision-long-range-infrared-distance-sensor-for-nxt-or-ev3 | website here]].Running some tests , the sensor does not provides good reading when the ball is < 150mm . So I will put a brick in 150mm from the sensor to avoid these wrong readings.
                  

 **# Attempt to use PID:**
 
 I will start only with PD control , and after i got a good response i will introduce the integral term to avoid the case where the motor does not rotate if the ball stop close to the set point distance.
     
 * __Proportional control:__ The bar will lift up or down as fast as how far is the ball from the setpoint.Power output command proportional to distance from sensor.
     
 * __Derivative control:__ to avoid oscillations from pure proportional control ( helps to brake the ball when close to setpoint).Power output command proportional to velocity ( d/dt distance).


 ** # Code Sketch **
 
 I'm thinking in something like this: ( i'm not sure if this works or not , it's just my first notes ). The motor will rotate faster when the ball is far from the setpoint (proportional) , and even faster if the ball is far from the setpoint and still moving (derivative).



   Define the variable : d_setpoint ( where the ball will balance )  



 Function to calculate the distance and the velocity of the ball.  
      
              
                  d_actual = sensor reading         // unity is mm 
                  error = d_actual – d_prev
                  dt= (currenttick – prevtick )     // unity is ms 
                  velocity = error/dt
                  d_prev= d_actual
                  prevtick=currenttick




               
Task to calculate the motor output to keep the balance

   kp= ... 
   kd=.... 
               while true
                   power= Kp*error + Kd*velocity
                     
                       if power <0    // as the OnFwd does not accept negative values for the power output
                          OnRev(port , ABS(power))      
                     
                       Else
                           OnFwd(port,power)      
                     end if 
                         
                     End while   


** #WEEK 2 REPORT **

After working on the code idea from week 1 , the final code flowchart is shown below:

{{::flow_ballandbeam.png?200|}}

** PROBLEMS:**

 ** Sensor Imprecision ** : Even with the ball stable , the distance measured by the sensor keeps changing ( See video below, the distance keeps changing around 490~~500 ).This floating on the distance induce a wrong velocity calculation ( The ball is stable but the velocity is not 0 ).

 * __Solution:__ Use the average from X distances instead of only one. At the first time i tried to measure 20 distances and get the average, but it takes a lot of time and the stability of the system is affected . I'm using the last 5 values , since it seems to not affect much the stability. However , the measures still floating around 5mm.

 {{youtube>QG-SVGAwWt4?medium}}



** TUNING THE GAINS : IS THE ALGORITHM CORRECT?**

**Proportional Control **: Using only the proportional control the system responses seems to be good , the system is showing the desired behavior . The motor only changes the rotation direction when the ball cross the set point distance. 

{{youtube>wEpdFiihg6I?medium}}

** Derivative Control **: After applying the filter in the distance measures ( still floating 5mm ) , the first step i took was to find a gain value for the derivative term that is low enough to ignore the floating when the ball is stable and high enough to be sensible and brake the ball when it's accelerating. 


** PD Control :** Since then i'm trying to tune the gains to make the system works smoothly, see video below. The system response still very slow , taking 20 seconds to reject a disturbance. I'm not sure if i can make it better tuning the gains or i should change the algorithm.

{{youtube>l0wOiOC10NE?medium}}
 

** PID Control :** I will apply the integral term after i can make the PD works. So the integral term will help on the case where the ball stops near to the setpoint , but not on it. ( the derivative term goes to 0 and the proportional term is very small ).


 
**ALGORITHM IDEA : **
 I'm using a PD (after will be PID) to get the ball distance from the set point. A serie of conditional statements makes the platform goes down and up depending on how far is the ball from the set point. After the motor get to the desired angle , it will go back to 0 deg.  

** NXT CODE **:

     
   // Files from the Long Range IR sensor
    #include "dist-nx-lib.nxc"
    #define DIST_ADDR 0x02

   task main() {
    // Set and Start the Sensor on Port1
    SetSensorLowspeed(S1);
    DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR);
    byte result;
    PosRegEnable(OUT_A);

   // Defining Variables
    float KP=1,KD=0.5;     // Proportional and Derivative Gains
    int position, dist, d_setpoint, i=0 , d_array[5];
    float velocity;
    int prevdist,d_test;
    float prevtick, dt;
    string positionstr,ptext,rawstr,rtext;

    // Preparing to enter the loop
     d_setpoint=460;
     prevdist=461;
     prevtick=0;


    // Control Loop
     while (true)
     {
     // The distance used is the average from 5 measures.
     while (i<5)
     {
     d_array[i]=(DISTNxReadValue(S1, DIST_REG_DIST_LSB, 2, DIST_ADDR)) ;
     i=i+1;
     }
     i=0;

     dist=ArrayMean(d_array,NA,NA);   // Distance is avg from 5 measures
     dt=CurrentTick()- prevtick;     // Time interval from each loop
     velocity=(dist-prevdist)/dt;    // Calculating Velocity
     velocity=velocity*1000;         //Converting to mm/s
     prevtick=CurrentTick();
     prevdist=dist;

    // ball position equation applying PD control:
     position = KP*(dist-d_setpoint) + KD*(velocity);

    // Show the raw distance and the position on the screen
     rawstr=NumToStr(dist);
     rtext=StrCat("raw:",rawstr);
     TextOut(10,LCD_LINE4,rtext);
     positionstr=NumToStr(position);
     ptext=StrCat("distance:", positionstr);
     TextOut(10,LCD_LINE2,ptext);

     // conditional statements to control the platform
     if (position >35 & position <45)
     {
     PosRegAddAngle(OUT_A,-20);
     Wait(100);
     }
     
     if (position >45 & position <55)
     {
     PosRegAddAngle(OUT_A,-21);
     Wait(100);
     }
     if (position >55 & position <65)
     {
     PosRegAddAngle(OUT_A,-23);
     Wait(100);
     }
     
     if (position >65 & position <75)
     {
     PosRegAddAngle(OUT_A,-22);
     Wait(100);
     }
     
     if (position >75 & position <85)
     {
     PosRegAddAngle(OUT_A,-25);
     Wait(100);
     }

     if (position >85 & position <100)
     {
     PosRegAddAngle(OUT_A,-30);
     }

     if (position >100 & position <300)
     {
     PosRegAddAngle(OUT_A,-65);
     Wait(80);
     }

     if (position >300)
     {
     PosRegAddAngle(OUT_A,-80);
     Wait(100);
     }

     // Especial conditions , close to the setpoint
     if (position <35 & position >-35)
     {
     PosRegSetAngle(OUT_A,0);
     Wait(150);
     }

     if (position <-35 & position >-45)
     {
     PosRegAddAngle(OUT_A,20);
     }

     if (position <-45 & position >-55)
     {
     PosRegAddAngle(OUT_A,25);
     Wait(100);
     }
     
     if (position <-55 & position >-65)
     {
     PosRegAddAngle(OUT_A,30);
     Wait(100);
     }
     
     if (position <-65 & position >-75)
     {
     PosRegAddAngle(OUT_A,35);
     Wait(100);
     }
     
     if (position <-75 & position >-85)
     {
     PosRegAddAngle(OUT_A,40);
     Wait(100);
     }

     if (position <-85 & position >-100)
     {
     PosRegAddAngle(OUT_A,45);
     }

     if (position <-100 & position >-300)
     {
     PosRegAddAngle(OUT_A,50);
     }

     if (position <-300)
     {
     PosRegAddAngle(OUT_A,60);
     Wait(100);
     }

     Wait(130);
     ClearScreen();
     PosRegSetAngle(OUT_A,0);  // Return the platform to 0 deg
     }      // end while
     }       // end task


** #WEEK 3 REPORT **:

 *1: I changed the algorithm to make the system run smoothly.

 *2: Derivation of the transfer function that represent the ball and beam system.

 *3: Simulation using Simulink and comparison with real system

 [[pid_vs_lqr_ballandbeam| WEEK 3 WORK ]]