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, 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.
NXT CODE :
// Sensor files
#include "dist-nx-lib.nxc"
#define DIST_ADDR 0x02
// PID GAINS
#define KP 3
#define KD 2
// 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
task main()
{
// Initialize and set the Long Range IR Sensor
SetSensorLowspeed(S1);
DISTNxSendCommand(S1, DIST_CMD_ENERGIZED, DIST_ADDR);
byte result;
// Defining variables
int power, dist, d_setpoint=480;
float velocity;
while(true)
{
// Get the Distance and Velocity of the ball from the function
calculatedistvelo(dist,velocity);
//Power equation using PD control:
power = KP*(dist-d_setpoint) + KD*(velocity);
// Power < 0 means the ball is going in direction to the sensor
// OnFwd makes the platform goes down ( to break the ball )
// 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)
{
OnRev(OUT_A,(power/15));
}
if (power < 0 )
{
OnFwd(OUT_A,abs((power/15)));
}
} // end while
} // end task
#WEEK 3 SCHEDULE:
*1: I'm planning to spend one more day trying to do the gain tune.
*2: Finish all the theoretical work: derive the transfer function and simulate it on Simulink to get the system parameters.
*3: Get back to the algorithm and think in something new
*4: Make the system run smoothly
*5: Include a File Saving function on the code, plot a disturbance rejection situation and compare it to the simulink model.