DASL Wiki

User Tools

Site Tools

Trace: • darwin_car • hitechnic_angle_encoder • lego_dynamixel_control_xl320 • how_podo_works_with_ros-joy
lego_ball_and_beam

Differences

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

Link to this comparison view

Next revision		Previous revision
lego_ball_and_beam [2016/01/25 23:02] – created joaomatoslego_ball_and_beam [2016/02/04 17:07] (current) – joaomatos
Line 1: Line 1:
-.+** #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 ]]
lego_ball_and_beam.1453791740.txt.gz · Last modified: by joaomatos