I´ll play around with just PD for a while. It´s a good thing for us newbies to reduce the tuning parameters and options when fumbling forward...
In all PID tuning seems to be an open field but you guys (zhomeslice and gciurpita) really have good and competent arguments, and even proof (Balancing Robot)!!
I´ll work on it and be back with a report.......
while not accurate (too ideal and too simple), i think the plot illustrates the effect of the PID derivative term.
it shows position (white), speed (red) and signed pwm motor setting (violet). Kp is 3.7, Kd is -1.5 (added to P) and hardly optimal.
target pos is 300. When starting, the motor is maxed out. But as speed increases and Kd has a greater affect, the motor control value, PWM, is reduced quickly. Approaching target causes it to reduce further. Reducing the motor PWM causes it to coast more than push. When it becomes negative (~500 msec), braking. The longer term plot (assuming a perfectly linear motor w/ no minimum operating voltage) results in the position wandering around the target of 300 +/- 2 along with the motor PWM becoming +/-.
the simulation is ideal because it doesn't model the motor very well and doesn't account for some minimum voltage to the motor for it to turn.f but the plot illustrates the effect of D term and that the I term is no necessary.
gciurpita:
while not accurate (too ideal and too simple), i think the plot illustrates the effect of the PID derivative term.it shows position (white), speed (red) and signed pwm motor setting (violet). Kp is 3.7, Kd is -1.5 (added to P) and hardly optimal.
target pos is 300. When starting, the motor is maxed out. But as speed increases and Kd has a greater affect, the motor control value, PWM, is reduced quickly. Approaching target causes it to reduce further. Reducing the motor PWM causes it to coast more than push. When it becomes negative (~500 msec), braking. The longer term plot (assuming a perfectly linear motor w/ no minimum operating voltage) results in the position wandering around the target of 300 +/- 2 along with the motor PWM becoming +/-.
the simulation is ideal because it doesn't model the motor very well and doesn't account for some minimum voltage to the motor for it to turn.f but the plot illustrates the effect of D term and that the I term is no necessary.
@gciurpita
Nice Example of PID when controlling speed and this could very well be an example of temperature control too.
P + I + D = output
P is equal to the error multiplied by Kp, so if the error is zero the output is zero. This is the key to positioning bots. rather than speed bots. when your on setpoint you are driving straight or in my balancing bot case we are at 0° like your bot you don't want to have more power on one motor over another so the call to turn is zero and my power to my balancing bot motor power is zero as don't want them to turn in either direction.
For speed control, If your target is 300 and you only used proportional only (in PID) at startup your error is 300 (For simplicity sake lets make Kp = 1) 300 * 1 = 300 output. now 300 (255 being the max PWM) we are at full power and the motors start moving fast! so as our speed increases the error decreases. let's say we reach 300 so our error is zero now when the error is zero our power to our motors is zero! Well, this isn't good, we can't ever reach setpoint with proportional only when speed is the desired control as our power is now zero when we reach setpoint. With proportional only we could reach say a speed of 182 for example (300 setpoint - 182 actual speed = an Error of 118) and our power to our motors is 118 (118 * 1 = 118). We need something to add to our power to get the speed from 182 up to 300 so we use integral. When the use integral starts ticking in, this is where duration is important if we don't have a timer here we will tick integral in fast and if our sample rate is to slow, the integral will be sluggish. I like to use a timer of 10 ms for my sample rate when controlling motors, with integral and derivative. (Proportional is unaffected by any duration). Every 10ms we are going to, for the sake of this example, use the error of 118 and multiply it by Ki (Ki = 0.1 for simplicity) then add it the integral output value winding it up. so the output gets 11.8 (118 8 0.1 = 11.8) the output is now powered at 129.8 thus affecting the speed and decreasing the effect of Kp all at about the same time. so speed increases Proportional decreases as we get closer to setpoint and integral adds a smaller amount at the next 10ms interval. We keep the lase 11.8 and add say 9 (new error * Ki), to get 20.8 for a new P+I+D=output that lands somewhere around 127 (proportional influence changed as well decreasing the output)
Derivative looks at the last error reading and compares it to the current error. The 10ms duration is important here also. too short of duration we don't see much change in the error and so noise becomes a huge factor. too large a duration we see huge influences in the derivative value added to the output. The derivative only added when we see a change between readings no chang no derivative influence huge change huge influence. I like to think of the derivative as a way to kick things back on track. Noise in the sensors readings becomes a huge factor when the derivative is applied. too much noise could cause your motors to chase ghosts.
My version of PID_v1 attached below considers the following:
The duration for the Ki and Kd multiplier is 1 second (how much influence per second) in my example we sample at 10ms so we add 1/10th the values of Ki and Kd to the output at each calculation. Kp is immune to duration as we just multiply the Kp with the error to get its output value.
tuning PID:
For Speed Control and not position control!
When controlling the speed (and temperature), start with integral and have the proportional and derivative set to zero Kp and Kd. We must reset the integral windup at startup so that the motors are at zero power and let the integral start winding up to achieve the speed desired. Ideally integral will slightly overshoot and then land on setpoint this will be slower than expected as Proportional and derivative are not in play at this time. After you get good integral control, add the proportional control Kp until it starts to oscillate then back off. add derivative until the speed starts to jitter then back off. integral can then be further adjusted to get a rapid and fast control reaction.
For Position Control and not speed control
Start with Proportional and find a point when it achieves setpoint without excessive oscillation, add derivative until jittering starts and back derivative off. Then if you desire, and you feel it's necessary, add some integral influence, be sure to reset integral offset value to zero (may require altering the PID routine) and do not release integral to start winding up until the positional control setpoint is reached (the error is zero) for the first time. With positioning control, you might find a dead zone around zero where the swing through zero of the motor power from forward to reverse (in my balancing bot example) has no influence. If your bot is moving in one direction already adjusting power to cause the bot to turn shouldn't experience this dead zone effect. (tighten up the linkage so to say to make sure the dead-zone is minimal if you want good PID control)
Z
Nice Example of PID when controlling speed
this was a sim of position control (i.e. no I)
gciurpita:
this was a sim of position control (i.e. no I)
Without integral would your simulation land on setpoint, or was it offset when it settled? I'm guessing without integral it would never reach setpoint.
Z
zhomeslice:
Without integral would your simulation land on setpoint, or was it offset when it settled?
target is 300
zhomeslice:
I'm guessing without integral it would never reach setpoint.
before I explain why, i'd like to hear your reasoning
greg
gciurpita:
target is 300
before I explain why, i'd like to hear your reasoninggreg
My Explanation:
P + I + D = Output
Error * Kp = P
For your Example, we are not using Integral so I = 0
The derivative is triggered when there is a change and only when there is a change. If we are maintaining speed and nothing is changing speed stays the same Derivative will have no effect and D = 0.
So ignoring Derivative and focusing on Proportional only
Fact: In order to maintain speed, you must provide some sort of power Greater than zero unless you are in a vacuum in space with no effects of gravity 
Fact: If you are at the desired setpoint, your Error is zero.
Using the Proportional equation (Error * Kp = P) zero times anything equals zero power to your motors so you will never reach the desired setpoint without Integral (Unless your desired output is known when you are on setpoint). it may get close or overshoot if proportional has too much influence, but we can never maintain speed for any duration of time.
Z
PS.
Somewhat restating how you might test speed control with your PID speed based Example.
Run your graph with Integral only no derivative chooses a value so that it will land on the desired setpoint.
change setpoint or create a disturbance and see how fast it recovers it should oscillate 3-4 times before landing. Integral is SLOW in responding. Adjust the Integral multiplier up or down to get the 3-4 oscillations.
Now add the proportional multiplier, then create a disturbance or change setpoint and see if the 3-4 oscillations occur less is good more is bad
get enough Proportional control so that it now oscillates once maybe twice
By adding Derivative we will eliminate the oscillation. Note: adding too much you will see jittering
you want it to snap onto setpoint no oscillations at all
Derivative acts like an air brake as we approach setpoint it robs power keeping the rate of change under control. The slower the rate of change the less Dirivitive has on the output. it can also add or remove power quickly when changes occur such as climbing a hill or coasting down a hill. The derivative will see the acceleration and quickly compensate before integral make too many adjustments helping to recover setpoint.
again, this sim controls position, not speed.
i agree that for speed control you need a residual output from PID to maintain a voltage to the motor to maintain speed when the error resulting from the P & D terms is zero.
could you explain why the integral is need for position control where once the target position is reached the motor should be stopped?
gciurpita:
again, this sim controls position, not speed.i agree that for speed control you need a residual output from PID to maintain a voltage to the motor to maintain speed when the error resulting from the P & D terms is zero.
could you explain why the integral is need for position control where once the target position is reached the motor should be stopped?
You don't need integral for position control.
I added it to enhance my balancing bot control. But it would have balanced without integral.
How would integral help in positioning.
I could see it help follow a curved wall or line integral would keep it right on setpoint but I would highly restrict the windup giving it gust a little wiggle room. It would act like a self alignment routine, adjusting the motors to give it enough offset to always hold setpoint.
With my balancing bot I would control motion forward and backwards by chanting the setpoint off of zero by about 5 degrees. Integral would help me maintain and recover from that adjustment. But when my bot fell over I would reset integral windup to zero and would not release integral until my bot was balancing again.
I would have everything else (P & D) tuned before considering adding any integral for position control.
Z
how do you deal with dead zones in motors?
You don't need integral for position control.
thumbs-up
gcjr:
how do you deal with dead zones in motors?
thumbs-up
From my earlier post, I shared this X Y to Tank Drive I use on my balancing bot Which would work great for the example project.
/*
* For this example I am using RC pulse which ranges from 0 to 1000 having th RC remote
* centered the output would be around 500. i shift this to provide a value from -500 to +500
* so X and Y will be looking for a value between +- 500
* In turn the output for the Left motor "LeftDrive" and right motor "RightDrive" will scale between +-500
* The Thottle value is a max power and should scale between 0 and 500
*/
int deadZone = 50;
void XYtoTank(int16_t X, int16_t Y, int16_t* LeftDrive, int16_t* RightDrive,int16_t Throttle){
int16_t LeftD,RightD,V;
float DriveScaler;
//mix throttle and direction
LeftD = (X - Y) ;
RightD = (X + Y) ;
DriveScaler = (max(1, max(abs((float)LeftD / 500.0), abs((float)RightD / 500.0))));
V = (int16_t)constrain((float)LeftD / DriveScaler, -500, 500);
if(deadZone)V = TankDeadzone(V, deadZone);
V = (Throttle == -1) ? V: map(V,0,500,0,Throttle); // Adjust Throttle Controlls Max Speed
*LeftDrive = V ;
V = (int16_t)constrain((float)RightD / DriveScaler, -500, 500);
if(deadZone)V = TankDeadzone(V, deadZone);
V = (Throttle == -1) ? V: map(V,0,500,0,Throttle); // Adjust Throttle Controlls Max Speed
*RightDrive = V;
TankLeftDrive = *LeftDrive;
TankRightDrive = *RightDrive;
}
int16_t TankDeadzone(int16_t V, uint16_t deadzone){
int16_t absV = abs(V);
absV = (absV < 3) ? 0 : map(absV, 3, 500, deadzone ,500) ; // Inserts a Deadzone
V = (V >= 0) ? absV : -absV; // Restores a FullRange Value +-
return(V);
}
The function TankDeadzone that follows assumes that you are crossing over zero from a positive value to a negative value with your PID loop. Instead of fiddling with the PID, I just feed the value after the PID calculation and turning calculations for each motor through this simple dead zone function
int16_t TankDeadzone(int16_t V, uint16_t deadzone){
int16_t absV = abs(V);
absV = (absV < 3) ? 0 : map(absV, 3, 500, deadzone ,500) ; // Inserts a Deadzone
V = (V >= 0) ? absV : -absV; // Restores a FullRange Value +-
return(V);
}
Steps:
It gets the power 0 to +-500 and converts it is the absolute value 0 to 500.
We now remove the dead zone and insure an off mode at setpoint (in order to transition powerful motors from forward to reverse, I use the output of zero around the PID output of +-3 for a breaking mode) then when the PID deviates from the off mode +-3 the motors go the minimum power or the dead zone value which is +-30 in my balancing bot case.
we now have 0, 30~500 power output.
because we are using an absolute value we need to convert it back to a +- value the next line restores the motor direction.
Now we have -500~-30, 0, +30~+500 for a power range for this motor.
Why +- 500 or a range of 1000, This is directly related to my RC remote which provides a pulse of 0 to 1000us where zero is down and 1000 is up on my controls. In this way, I can bypass my PID control and directly control the motors if I desire.
I later convert the +-500 to an absolute value for the PWM signal and map it to 0-255 and the +- gets converted to a forward or reverse command for the H-Bridge controller.
In the case of a bot that is moving forward, only a small differential in power to the motors is needed to turn. The dead zone may be minimal to none for the required amount of power difference to achieve the turn. If the bot is stopped a dead zone would likely be needed to get a snappy response.
Z
I use the output of zero around the PID output of +-3 for a breaking mode
i've been thinking about this braking approach. by braking, i assume you mean shorting the motor thru the h-bridge
it adds a non-linear component to the linear PID algorithm which slightly complicates the code. The effect is proportional to speed. so i doubt braking when the motor is going slow has any effect.
where i think this is likely to become relevant during position control is at some distance from the target where the P and D (speed) terms balance (cancel one another) and the desire is to maintain that speed. It's likely that subsequent iterations would push the motor voltage negative which would actively brake regardless of speed.
so it seems that the dead zone should be handled by recognizing the minimum voltage to the motor and when the PID output swings from >0 to <0 to swing the motor voltage from minimum positive, thru zero (when PID == 0) to minimum negative (step change).
i wouldn't say this makes PID non-linear, it makes the motor control mapping PID output to motor voltage non-linear.
on the other hand, perhaps a non-PID approach could use braking while the motor is at speed if a good estimate of the deceleration rate were known (obtained during a calibration sweep). if you know that it takes so much distance to come to a stop from a specific speed, you could apply braking at that speed and distance. It's conceivable that braking is modulated (on/off) as you approach the target (like spoilers on a glider) so apply braking early.
if you come up short or long, i wonder if pulsing the motor could change position sufficient to reach the target +/- some tolerance. is one motor revolution acceptable tolerance?
thanks for your explanation
greg
gcjr:
i've been thinking about this braking approach. by braking, i assume you mean shorting the motor thru the h-bridgeit adds a non-linear component to the linear PID algorithm which slightly complicates the code. The effect is proportional to speed. so i doubt braking when the motor is going slow has any effect.
where i think this is likely to become relevant during position control is at some distance from the target where the P and D (speed) terms balance (cancel one another) and the desire is to maintain that speed. It's likely that subsequent iterations would push the motor voltage negative which would actively brake regardless of speed.
so it seems that the dead zone should be handled by recognizing the minimum voltage to the motor and when the PID output swings from >0 to <0 to swing the motor voltage from minimum positive, thru zero (when PID == 0) to minimum negative (step change).
i wouldn't say this makes PID non-linear, it makes the motor control mapping PID output to motor voltage non-linear.
on the other hand, perhaps a non-PID approach could use braking while the motor is at speed if a good estimate of the deceleration rate were known (obtained during a calibration sweep). if you know that it takes so much distance to come to a stop from a specific speed, you could apply braking at that speed and distance. It's conceivable that braking is modulated (on/off) as you approach the target (like spoilers on a glider) so apply braking early.
if you come up short or long, i wonder if pulsing the motor could change position sufficient to reach the target +/- some tolerance. is one motor revolution acceptable tolerance?
thanks for your explanation
greg
Greg,
Breaking is minimal compared to powering up the motor in reverse and letting the motor cost with no power is potentially a loss of control. with my Balancing bot, consider that it only breaks when we are calling for literally 0 when swinging from forward 30 pwn to Reverse 30 PWM through the H-Bridge. At 30 PWM my motors on my balancing bot hum and possibly try to move in the direction I'm requesting this power level is definitely breaking for the opposite direction that I'm calling for. For 6 steps through setpoint (+-3) which is relatively small compared by the full control range of +-500 (which is a floating-point number with enough detail that because we are at setpoint we could float around the +-3 for some time). By hindering movement in both directions using the H-Bridge breaking mode seem to me to be a more accurate representation of what I am desiring. Consider driving a balancing bot up a hill then attempting to stop, would you rather release the motors to coast when you are balanced or cause some resistance by applying braking to the H-Bridge. Of course, balancing on a flat surf if you are moving forward you may want to coast while balanced. I tend to find breaking a better option here as breaking will slow the balancing bot towards stopping possibly helping me maintain a position.
This breaking/no-breaking, by the way, has come about with many many tests, I seem to like breaking but I have great control without breaking as well.
Here is my H-Bridge "TankPWM" code for my balancing bot and all Tank-style 2 motor steering Bots I have:
/* TankPWM *
LeftDrive: PWM Value +-500
RightDrive: PWM Value +-500
Breaking: Breaking Level 0 ~ 255
LeftPin, Forward_LeftPin, Reverse_LeftPin, RightPin, Forward_RightPin, Reverse_RightPin :
are the pins on the Uno that are being used to control the H-Bridge
*/
void TankPWM(int16_t LeftDrive,int16_t RightDrive, int16_t Breaking, uint8_t LeftPin, uint8_t Forward_LeftPin , uint8_t Reverse_LeftPin, uint8_t RightPin, uint8_t Forward_RightPin, uint8_t Reverse_RightPin ){
static int16_t LastLeftDrive;
static int16_t LastRightDrive;
static int16_t LastLeftDriveDirection;
static int16_t LastRightDriveDirection;
static int16_t ActualLeftDrive;
static int16_t ActualRightDrive;
ActualLeftDrive = abs(LeftDrive);
ActualRightDrive = abs(RightDrive);
if ((LeftDrive == 0) ||((LastLeftDriveDirection > 0) != (LeftDrive > 0))){
digitalWrite(Forward_LeftPin, LOW);
digitalWrite(Reverse_LeftPin, LOW);
ActualLeftDrive = min(255,abs(Breaking));
}
if ((RightDrive == 0) ||((LastRightDriveDirection > 0) != (RightDrive > 0))){
digitalWrite(Forward_RightPin, LOW);
digitalWrite(Reverse_RightPin, LOW);
ActualRightDrive = min(255,abs(Breaking));
}
//todo Add Slow Ramp up to Prevent sudden changes in direction for more powerful motors
LastLeftDriveDirection =LeftDrive;
LastRightDriveDirection= RightDrive;
LastLeftDrive = abs(LeftDrive);
LastRightDrive = abs(RightDrive);
analogWrite(LeftPin,constrain(map(ActualLeftDrive,0,500,0,255),0,255));
analogWrite(RightPin,constrain(map(ActualRightDrive,0,500,0,255),0,255));
if(LeftDrive < 0){
digitalWrite(Forward_LeftPin, HIGH);
digitalWrite(Reverse_LeftPin, LOW);
}
if(RightDrive < 0){
digitalWrite(Forward_RightPin, HIGH);
digitalWrite(Reverse_RightPin, LOW);
}
if(LeftDrive > 0){
digitalWrite(Forward_LeftPin, LOW);
digitalWrite(Reverse_LeftPin, HIGH);
}
if(RightDrive > 0){
digitalWrite(Forward_RightPin, LOW);
digitalWrite(Reverse_RightPin, HIGH);
}
}
Z
P.S.
@ dwnld99
Thanks for letting us steal your post I hope this is helpful with your project as it is relevant is many ways to controlling a positioning bot.
It´s absolutely OK zhomeslice! I´m still reading and trying to catch up - but sometimes you are at a much higher level than I am. It´s still interesting for me though to learn and understand PID, and be able to at least tune the PID library.
To go back to what I was trying from the beginning I attached a little video, very compressed to fit 2MB, and not near your balancing robot in technical beauty!!
This is an old video but the bot is similar to the one I use now. The simple goal in the video was to complete the lane without hitting or get stuck at the walls. There´s no PID working in the video, just simple while loops with static reactions to preset distances - hence the jerky driving! As I remember it was only the right distance sensor giving input in the video - similar to what I´m trying with PID now.
In the "First try with PID and PID library" post I just wanted to see if it was possible get a smoother ride with PID than in the attached video - and now just following a single wall at the right hand.
The biggest problem I seems to have is that the angle when passing setpoint coming from >setpoint is not 90 degrees towards the wall (straight forward) making the measured distance at that point larger than setpoint (eg 10cm) and making PID (or PD) pushing even more to reach setpoint and finally (or abrupt) bumping the bot into the wall. To prevent this I need a much tighter and faster control at setpoint with less drifting from it. Altough trying many different PID or PD setting I haven´t been able to achieve this. Maybe just measuring with one distance sensor towards the wall is not enough?
Thomas
Untitled.mpg (1.49 MB)
dwnld99:
It´s absolutely OK zhomeslice! I´m still reading and trying to catch up - but sometimes you are at a much higher level than I am. It´s still interesting for me though to learn and understand PID, and be able to at least tune the PID library.To go back to what I was trying from the beginning I attached a little video, very compressed to fit 2MB, and not near your balancing robot in technical beauty!!
This is an old video but the bot is similar to the one I use now. The simple goal in the video was to complete the lane without hitting or get stuck at the walls. There´s no PID working in the video, just simple while loops with static reactions to preset distances - hence the jerky driving! As I remember it was only the right distance sensor giving input in the video - similar to what I´m trying with PID now.
In the "First try with PID and PID library" post I just wanted to see if it was possible get a smoother ride with PID than in the attached video - and now just following a single wall at the right hand.
The biggest problem I seems to have is that the angle when passing setpoint coming from >setpoint is not 90 degrees towards the wall (straight forward) making the measured distance at that point larger than setpoint (eg 10cm) and making PID (or PD) pushing even more to reach setpoint and finally (or abrupt) bumping the bot into the wall. To prevent this I need a much tighter and faster control at setpoint with less drifting from it. Altough trying many different PID or PD setting I haven´t been able to achieve this. Maybe just measuring with one distance sensor towards the wall is not enough?
Thomas
Tomas,
Nice project
I through some code together to give you some ideas, an example. It compiles and should work, although I haven't tested it.
it has a basic proportional control routine, it uses 2 ping sensors that must be hooked to pins 2 and 3, as I use interrupts to handle the ping time. I have the PWM output for your motors on Pins 9 and 10. It averages 10 pulses together to get the reading for the Proportional turn influence. It calculates the proportional change 10 times a second and the code is not blocking.
I wish I had a bot to try it no now.
float Kp = 0.9; // Adjust the Proportional influance this is just a guess
float Setpoint = 5; // cm
int Speed = 110; // 0-255 drive forward speed.
#define oneMeter 5800 // microseconds round trip
#define PWMPinA 9
#define PWMPinB 10
float microsecondsToInches(long microseconds)
{
return (float) microseconds / 74 / 2;
}
float microsecondsToCentimeters(long microseconds)
{
return (float)microseconds / 29 / 2;
}
volatile byte AvgPos[2];
volatile unsigned long PingTimes[2][10]; // Pins 0 and 1 are ignored we are wasting 2 longs of memory
void PingItPin2() {
PingIT(2, 0);
}
void PingItPin3() {
PingIT(3, 1);
}
// This function fills an array of 10 Ping readings for up to 2 Ping Sensors
// The array is filled with First in First out. FIFO buffer which we will add and divide by 10
void PingIT(int PingPin, int StorageSlot) {
static unsigned long EdgeTime[2];
unsigned long cTime = micros();
if (digitalRead(PingPin)) {
EdgeTime[StorageSlot] = cTime; //Pulse went HIGH store the start time
} else { // Pulse Went low calculate the duratoin
AvgPos[StorageSlot]++; // Incraments AvgPos for desired ping sensor
AvgPos[StorageSlot] = AvgPos[StorageSlot] % 10; // uses remainder % after dividing by 10 to count from 0 to 9
PingTimes[StorageSlot][AvgPos[StorageSlot]] = min(cTime - EdgeTime[StorageSlot], oneMeter); // Calculate the change in time
}
}
void setup() {
// put your setup code here, to run once:
Serial.begin(115200); //115200
while (!Serial);
//*** use pins 2 and 3 for your inputs from you ultrasonic Sensors.
attachInterrupt(digitalPinToInterrupt(2), PingItPin2, CHANGE);
attachInterrupt(digitalPinToInterrupt(3), PingItPin3, CHANGE);
}
void loop() {
// put your main code here, to run repeatedly:
static unsigned long PingTriggerTimer;
if ( millis() - PingTriggerTimer >= (5)) { // creates a 5 ms pulse every 5 ms which will work to retrigger the ultrasonic ping sensors
PingTriggerTimer += (5);
// you can trigger the ultrasonic ping sensor as fast as you want it won't send a new signal until it recieves an echo or times out.
static int triggerCtr;
triggerCtr++;
digitalWrite(4, triggerCtr % 1); // %1 will return 1 or 0 for odd or even number
digitalWrite(5, triggerCtr % 1); // %1 will return 1 or 0 for odd or even number
}
static unsigned long PIDTimer; // actually Proportional only for now.
int Delay = 100;
if ( millis() - PIDTimer >= (Delay)) {
PingTriggerTimer += (Delay);
unsigned long SumTimeA, SumTimeB, AvgTimeA, AvgTimeB;
for (int i = 0; i < 10; i++) {
noInterrupts ();
SumTimeA += PingTimes[0][i];
SumTimeB += PingTimes[1][i];
interrupts ();
}
AvgTimeA = SumTimeA * .1;
AvgTimeB = SumTimeB * .1;
float DistanceCMforA, DistanceCMforB;
DistanceCMforA = microsecondsToCentimeters(AvgTimeA );
DistanceCMforB = microsecondsToCentimeters(AvgTimeB );
Serial.print(DistanceCMforA, 2);
Serial.print("cm\t");
Serial.print(DistanceCMforB, 2);
Serial.println("cm");
bool TurnRight = DistanceCMforA > DistanceCMforB;
float MinDistance = min(DistanceCMforA , DistanceCMforB);
float Turn;
float output;
output = (MinDistance - Setpoint) * (Kp * (Delay * 0.001)); // Proportional with Kp = ammount of influance per second so changing the duration wont affect you Kp multiplier
output = (TurnRight) ? -output : output;
int PWMLeftMotor;
int PWMRightMotor;
PWMLeftMotor = constrain (Speed + (int)output, 0, 255); //Add turn influance to left Motor
PWMRightMotor = constrain (Speed - (int)output, 0, 255); // Subtract the same amount from the right Motor
analogWrite(PWMPinA, PWMLeftMotor);
analogWrite(PWMPinB, PWMRightMotor);
}
}
Z
Ha ha - you are incredible zhomeslice! Thanks!! 
You "throw some code together" but at my level I have to examine it a bit to understand - but I will for sure try to test it!!
Just to sort some things out - you now refer to the video with two walls, one on each side of the bot (of course ), and then also two distance sensors left and right as on the bot in the video??
I´ll give it a try - hopefully later today!!
Thanks again!!
dwnld99:
Ha ha - you are incredible zhomeslice!You "throw some code together" but at my level I have to examine it a bit to understand - but I will for sure try to test it!!
Just to sort some things out - you now refer to the video with two walls, one on each side of the bot (of course
I´ll give it a try - hopefully later today!!
Thanks again!!
Yes the video with Two walls, you will only want to follow one at a time and use proportional only at least to start. Using the closest wall to maintain a setpoint will be easiest. whichever wall it sees first it needs to lock on and follow.
The averaging will allow for the bot to be controlled smoothly. and pinging fast is not a problem when you use interrupts.
Good Luck and let me know how it goes.
If you have questions about the code just ask, I'll keep an eye out for your post.
Z
I haven't had time to try this yet zhomeslice - back then it was christmas holiday, now just work.... 
Right now I have to bring another older (newbie ) thread to life in the forum......coming soon....