Here's the code, I hope it's useful.
/*BFF motion software reader and driver for Arduino v1.0 by R Cummins.
Even pins are for actuator 'extend' and odd pins are for actuator 'retract'.
Credit to Brett Beauregard for his excellent PID library. Some code also by
Brett Beauregard from his PID adaptive tuning example. Code is free to use.
Remember code must be tuned to whatever device(s) you're attempting to control.
Enjoy!
*/
#include <PID_v1.h>
int cr, count=1, pin, c, time=5, retlimit=150, exlimit=850;
int sread[9];
//Define Variables we'll be connecting to
double Input, Output1, Output2, Output3, Output4, Output5, Output6, Setpoint, gap;
//Define the aggressive and conservative Tuning Parameters
double aggKp=1.2, aggKi=0.070, aggKd=0.365;
double consKp=0.125, consKi=0.015, consKd=0.08;
//Specify the links and initial tuning parameters
PID act1PID(&Input, &Output1, &Setpoint, consKp, consKi, consKd ,DIRECT);
PID act2PID(&Input, &Output2, &Setpoint, consKp, consKi, consKd ,DIRECT);
PID act3PID(&Input, &Output3, &Setpoint, consKp, consKi, consKd ,DIRECT);
PID act4PID(&Input, &Output4, &Setpoint, consKp, consKi, consKd ,DIRECT);
PID act5PID(&Input, &Output5, &Setpoint, consKp, consKi, consKd ,DIRECT);
PID act6PID(&Input, &Output6, &Setpoint, consKp, consKi, consKd ,DIRECT);
void setup()
{
Serial.begin(9600);
for (c=2; c<14; c++)
{
pinMode(c, OUTPUT);
}
act1PID.SetMode(AUTOMATIC);
act1PID.SetSampleTime(time);
act2PID.SetMode(AUTOMATIC);
act2PID.SetSampleTime(time);
act3PID.SetMode(AUTOMATIC);
act3PID.SetSampleTime(time);
act4PID.SetMode(AUTOMATIC);
act4PID.SetSampleTime(time);
act5PID.SetMode(AUTOMATIC);
act5PID.SetSampleTime(time);
act6PID.SetMode(AUTOMATIC);
act6PID.SetSampleTime(time);
}
void loop()
{
if (Serial.available()>0)
{
cr=Serial.read();
if (cr==65)
{
do
{
if (Serial.available()>0)
{
sread[count]=(Serial.read());
count++;
}
}
while(count<9);
}
}
if (sread[1]==66)
{
//--------------------------------------------------actuator 1
Setpoint=sread[3];
//Setpoint=100;
Setpoint=map(sread[3], 0, 255, retlimit, exlimit);
//Setpoint=((Setpoint+1)*2.35)+100;
Input = analogRead(0);
if (Setpoint > Input)
{
act1PID.SetControllerDirection(DIRECT);
analogWrite(3, LOW);
pin=2;
}
else
{
act1PID.SetControllerDirection(REVERSE);
analogWrite(2, LOW);
pin=3;
}
gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<6)
{ //we're close to setpoint, use conservative tuning parameters
act1PID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
act1PID.SetTunings(aggKp, aggKi, aggKd);
}
act1PID.Compute();
if (gap<2)
{
Output1=0;
}
analogWrite(pin, Output1);
//--------------------------------------------------actuator 2
Setpoint=sread[4];
Setpoint=map(sread[4], 0, 255, retlimit, exlimit);
Input = analogRead(1);
if (Setpoint > Input)
{
act2PID.SetControllerDirection(DIRECT);
analogWrite(5, LOW);
pin=4;
}
else
{
act2PID.SetControllerDirection(REVERSE);
analogWrite(4, LOW);
pin=5;
}
gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<6)
{ //we're close to setpoint, use conservative tuning parameters
act2PID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
act2PID.SetTunings(aggKp, aggKi, aggKd);
}
act2PID.Compute();
if (gap<2)
{
Output2=0;
}
analogWrite(pin, Output2);
//--------------------------------------------------actuator 3
Setpoint=sread[5];
Setpoint=map(sread[5], 0, 255, retlimit, exlimit);
Input = analogRead(2);
if (Setpoint > Input)
{
act3PID.SetControllerDirection(DIRECT);
analogWrite(7, LOW);
pin=6;
}
else
{
act3PID.SetControllerDirection(REVERSE);
analogWrite(6, LOW);
pin=7;
}
gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<6)
{ //we're close to setpoint, use conservative tuning parameters
act3PID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
act3PID.SetTunings(aggKp, aggKi, aggKd);
}
act3PID.Compute();
if (gap<2)
{
Output3=0;
}
analogWrite(pin, Output3);
//--------------------------------------------------actuator 4
Setpoint=sread[6];
Setpoint=map(sread[6], 0, 255, retlimit, exlimit);
Input = analogRead(3);
if (Setpoint > Input)
{
act4PID.SetControllerDirection(DIRECT);
analogWrite(9, LOW);
pin=8;
}
else
{
act4PID.SetControllerDirection(REVERSE);
analogWrite(8, LOW);
pin=9;
}
gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<6)
{ //we're close to setpoint, use conservative tuning parameters
act4PID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
act4PID.SetTunings(aggKp, aggKi, aggKd);
}
act4PID.Compute();
if (gap<2)
{
Output4=0;
}
analogWrite(pin, Output4);
//--------------------------------------------------actuator 5
Setpoint=sread[7];
Setpoint=map(sread[7], 0, 255, retlimit, exlimit);
Input = analogRead(4);
if (Setpoint > Input)
{
act5PID.SetControllerDirection(DIRECT);
analogWrite(11, LOW);
pin=10;
}
else
{
act5PID.SetControllerDirection(REVERSE);
analogWrite(10, LOW);
pin=11;
}
gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<6)
{ //we're close to setpoint, use conservative tuning parameters
act5PID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
act5PID.SetTunings(aggKp, aggKi, aggKd);
}
act5PID.Compute();
if (gap<2)
{
Output5=0;
}
analogWrite(pin, Output5);
//--------------------------------------------------actuator 6
Setpoint=sread[8];
Setpoint=map(sread[8], 0, 255, retlimit, exlimit);
Input = analogRead(5);
if (Setpoint > Input)
{
act6PID.SetControllerDirection(DIRECT);
analogWrite(13, LOW);
pin=12;
}
else
{
act6PID.SetControllerDirection(REVERSE);
analogWrite(12, LOW);
pin=13;
}
gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<6)
{ //we're close to setpoint, use conservative tuning parameters
act6PID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
act6PID.SetTunings(aggKp, aggKi, aggKd);
}
act6PID.Compute();
if (gap<2)
{
Output6=0;
}
analogWrite(pin, Output6);
}
count=1;
}