User Tools

Site Tools


lego_ball_and_beam

This is an old revision of the document!


#WEEK 1 REPORT

# 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) 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:

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.

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.

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.

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 SCHEDULE:

*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

WEEK 3 WORK

lego_ball_and_beam.1454632654.txt.gz · Last modified: 2016/02/04 16:37 by joaomatos