Differences

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

--- lego_ball_and_beam [2016/01/31 20:29] – joaomatos
+++ lego_ball_and_beam [2016/02/04 17:07] (current) – joaomatos
@@ Line 63: / Line 63: @@
 After working on the code idea from week 1 , the final code flowchart is shown below:
-{{:flow_final.png?200|}}
+{{::flow_ballandbeam.png?200|}}
 ** PROBLEMS:**
@@ Line 84: / Line 84: @@
-** PD Control :** Since then i'm trying to tune the gains to make the system works smoothly, but i can't do it. Analyzing the video below , it feels like the system is not fast enough to brake the ball when it's accelerating from one end to another of the platform ( the motor takes too long to rotate when the ball is moving around the set point distance , so the ball overshoot it everytime ). I'm not sure if this is gain problem or the algorithm is wrong.
+** 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>i7qcr_2QfYA?medium}}
+{{youtube>l0wOiOC10NE?medium}}
-** NEW ALGORITHM IDEA : **
- Use a PID to compute the ball position and instead of using OnFwd and OnRev use angle rotation.
-** NXT CODE (NOT WORKING PROPERLY)**:
+** 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 ).
-     // Sensor files
-      #include "dist-nx-lib.nxc"
-      #define DIST_ADDR 0x02
-     // PID GAINS
+**ALGORITHM IDEA : **
-      #define KP 3
+ 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.
-      #define KD 2
+** NXT CODE **:
-     // A function that calculates the distance and the velocity.
-      void calculatedistvelo(int &dist, float &velocity)
-      {
-       int i,prevdist=481,d_array[5];
-       float prevtick, dt;
-      // 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;
-      }   // end calculate distance and velocity
+   // Files from the Long Range IR sensor
+    #include "dist-nx-lib.nxc"
+    #define DIST_ADDR 0x02
-    task main()
+   task main() {
-    {
+    // Set and Start the Sensor on Port1
-    // Initialize and set the Long Range IR Sensor
+    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);
+     }
-     SetSensorLowspeed(S1);
+     if (position >45 & position <55)
-     DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR);
+     {
-     byte result;
+     PosRegAddAngle(OUT_A,-21);
+     Wait(100);
+     }
+     if (position >55 & position <65)
+     {
+     PosRegAddAngle(OUT_A,-23);
+     Wait(100);
+     }
-    // Defining variables
+     if (position >65 & position <75)
+     {
+     PosRegAddAngle(OUT_A,-22);
+     Wait(100);
+     }
-     int power, dist, d_setpoint=480;
+     if (position >75 & position <85)
-     float velocity;
-     while(true)
      {
+     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);
+     }
-     // Get the Distance and Velocity of the ball from the function
+     if (position <-55 & position >-65)
+     {
-     calculatedistvelo(dist,velocity);
+     PosRegAddAngle(OUT_A,30);
+     Wait(100);
+     }
-     //Power equation using PD control:
+     if (position <-65 & position >-75)
+     {
+     PosRegAddAngle(OUT_A,35);
+     Wait(100);
+     }
-     power = KP*(dist-d_setpoint) + KD*(velocity);
+     if (position <-75 & position >-85)
+     {
-     // Power < 0 means the ball is going in direction to the sensor
+     PosRegAddAngle(OUT_A,40);
-     // OnFwd makes the platform goes down ( to break the ball )
+     Wait(100);
-     // Power >0 0 means the ball is going in direction to the motor
+     }
-     // OnRev makes the platform goes up ( to break the ball )
-     if (power > 0)
+     if (position <-85 & position >-100)
      {
-     OnRev(OUT_A,(power/15));
+     PosRegAddAngle(OUT_A,45);
      }
-    if (power < 0 )
+     if (position <-100 & position >-300)
      {
-     OnFwd(OUT_A,abs((power/15)));
+     PosRegAddAngle(OUT_A,50);
+     }
+     if (position <-300)
+     {
+     PosRegAddAngle(OUT_A,60);
+     Wait(100);
      }
-     }  // end while
-     }  // end task
+     Wait(130);
+     ClearScreen();
+     PosRegSetAngle(OUT_A,0);  // Return the platform to 0 deg
+     }      // end while
+     }       // end task
-** #WEEK 3 SCHEDULE**:
- *1: I'm planning to spend one more day trying to do the gain tune.
+** #WEEK 3 REPORT **:
- *2: Finish all the theoretical work: derive the transfer function and simulate it on Simulink to get the system parameters.
+ *1: I changed the algorithm to make the system run smoothly.
- *3: Get back to the algorithm and think in something new
+ *2: Derivation of the transfer function that represent the ball and beam system.
- *4: Make the system run smoothly
+ *3: Simulation using Simulink and comparison with real system
- *5: Include a File Saving function on the code, plot a disturbance rejection situation and compare it to the simulink model.
+ [[pid_vs_lqr_ballandbeam| WEEK 3 WORK ]]