Sixth day

Batteries and their charger arrived today, motors and ESC’s were out of stock, so we’ll have to wait about 2 weeks. Still better than ordering them from another country. Batteries were already tested and they work perfectly.

Image

The PCB mother board is still in development.

First aproach of a PID algorithm completed:

/*
PID control program by Carlos Giudice.
you can find the References section below the code.

*/

void setup()
{
analogReference(DEFAULT);
pinMode (3, OUTPUT);
Serial.begin(115200);
}

void loop()
{
actuate(PID(calculateError(getSetpoint(), getFeedback())));

}

//setpoint(1) data acquisition

float getSetpoint()
{
int setpointPin = 1;
float setpoint = analogRead(setpointPin);
return setpoint;
}

//feedback(2) data acquisition

float getFeedback()
{
int feedbackPin = 5;
float feedback = analogRead(feedbackPin);
feedback = constrain(feedback, 750, 1023);
feedback = map(feedback, 750, 1023, 1023, 0);

return feedback;

}

//calculate error(3)
float calculateError(float setpoint, float feedback)
{
float error;
static int skip = 0;
error = setpoint – feedback;
skip++;
if(skip==50)
{
skip=0;
Serial.print(“setpoint:”);
Serial.print(setpoint);
Serial.print(” | feedback:”);
Serial.print(feedback);
Serial.print(” | error:”);
Serial.print(error);
}
return error;
}

//PID function===>returns output
float PID(float error)                //IMPORTANT: set the gain values acording to your own criteria and needs.
{
float pGain = 5.0;
float iGain = 1.0;
float dGain = 2.0;
float output;                // the output needs to be scaled or constrained to 0-255 as those are the PWM output values
float pOut = proportional(error, pGain);
float iOut = integral(error, iGain);
float dOut = differential(error, dGain);
static int skip = 0;
output = pOut + iOut + dOut;
skip++;
if(skip == 50)  //we’re only printing data every 50 loops.
{
skip = 0;
Serial.print(” | proportional:”);
Serial.print(pOut);
Serial.print(” | integral:”);
Serial.print(iOut);
Serial.print(” | differential:”);
Serial.print(dOut);
Serial.print(” | output:”);
Serial.println(output);
}
return output;

}

//proportional term: bases output on current readings of the error.
float proportional( float Error, float pGain)
{
float pOutput;
pOutput = Error * pGain;    //calculate de proportional part of the output

return pOutput;
}

//integral term: bases output on past readings, as it remembers them.
float integral(float Error, float iGain)
{
float iOutput;
static float iState = 0;    //sum of the past errors
float iMax = 255;      //max value of iState
float iMin = -255;      //min value of iState

iState += Error;    //update iState
iState = constrain(iState, iMin, iMax);  //constrain iState to avoid windup(4)

iOutput = iState * iGain;    //calculate the integral output
return iOutput;
}

//differential function: bases output on future error predictions, wich are calculated with the current variation of error.
float differential(float Error, float dGain)
{
static float lastError = 0;
float errorDifference = Error – lastError;  //error variation
float dOutput = dGain * errorDifference;    //calculate the differential output
lastError = Error;       //save the current error to reuse it next time
return dOutput;

}

//actuate
void actuate(float finalOutput)
{
finalOutput = constrain(finalOutput, 0, 1023); //we need to fit the output in a 0-255 range
map(finalOutput, 0, 1023, 0, 255);
analogWrite(3, finalOutput);
//Serial.print(“finalOutput:”);
//Serial.println(finalOutput);

}
/*

REFERENCES:
1)Setpoint: the desired value, this is what we want the system to be like.
2)Feedback: the real Value, this is what the system really is.
3)Error (Setpoint-Feedback): the difference between the desired state and the real state. we want this parameter to be 0.
4)windup: excesive growth of the iState variable (i.e. remembers to much error), when desired state is reached,
windup can keep output really high and produce an unstable system. Also if the user changes the setpoint, it will produce a slow reaction.

*/

Keep in mind that this is a first approach, many changes must be done in order to have good algorithm. This code was tested on a system wich has a LED as actuator and an LDR as feedback. Setpoint comes from a potentiometer. This system proved to be really fast and no usefull data about what was happening could be extracted.

Well, that’s all for today, have a nice week.

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s