DASL Wiki

User Tools

Site Tools

Trace:
lego_ball_and_beam

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
lego_ball_and_beam [2016/01/25 23:18] – joaomatoslego_ball_and_beam [2016/02/04 17:07] (current) – joaomatos
Line 6: Line 6:
    
  **# Sensor limitations:**        **# Sensor limitations:**      
- I got the Syntax to get the reading of this sensor in the seller 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). 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. + 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. 
- # code sketch:                     +                  
  
  **# Attempt to use PID:**  **# Attempt to use PID:**
    
- Researching on the internet i found that the integral term is not necessary. ( possible to see after obtaining the transfer function of the B&B system , the integral term will add another integrator , making the system more unstable).+ 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.  * __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 break the ball when close to setpoint).Power output command proportional to velocity ( d/dt distance).+ * __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).
  
  
Line 59: Line 59:
  
  
 +** #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 ]]
lego_ball_and_beam.1453792713.txt.gz · Last modified: by joaomatos