After researching a lot, I finally stumbled on some simple PID code that I think makes sense to me. I am attaching the entire code so far (there will be some mistakes in it and unlinked stuff as I am just starting to put everything together - I don't really need help with that part, it will be corrected once all the pieces are together.)
Some background: The 10 motors are using simple H-bridges and the inputs are Forward and Reverse. Each one has a feedback sensor (some are pots and some are encoders). I have all the code working for reading the positions from the different feedback sensors and I am making them all conform to 0 to 255. I probably don't have all that code in here yet, but that part is working like a charm.
But for the purposes of the PID and PWM routines, I will post those snippets here:
/**********************************************************
PID Algorithm to determine the speed of the motors to arrive
at the desired position smoothly
*********************************************************/
int PID(){
P = currpos - pos;
I = I + P;
D = P - Last_P;
Last_P = P;
error = int(P * Kp + I * Ki + D *Kd);
constrain (error, -255,255);
return error;
if (error < 0){
DirLR = 1; //Right direction
speed = max_speed + error;
}
else{ //Where is the test for when error is zero? Seems I will be running this code when it is, causing an error and forcing the < 0 error to kick in again to correct
DirLR = 0; Left direction
speed = max_speed - error;
}
And now, the PWM code:
/**********************************************************
Simple PWM routine that will apply a pulse width to the pin
Period is 8ms so this code needs to be called every 8ms
or 125Hz.
tH values are 8 steps of 1ms each (or 12.5% duty cycle)
*********************************************************/
void PWM(byte tH, byte pin){
if tH >9{
digitalWrite(pin, HIGH);
delay(1*tH);
digitalWrite(pin, LOW);
}
}
The reason I need a simple psuedoPWM routine is that I need to control PWM on 20 different lines. The questions I have are:
- Do I need to provide different Kp, Ki, and Kd values for each motor? Probably.
- Is PID overkill in the first place?
- Does that PID code make sense?
- Looking for suggestions on how to implement a simple PWM routine. The one I have here is terrible. I will be running a delay for up to 8ms!
Some information probably needed for the PWM stuff is: The way the motors are currently controlled (by a controller I am replacing) is a PWm with a frequency of 125Hz. That's a period of 8ms. Within the 8ms period, there are 8 possible pulse widths of 1ms each. Some movements may take as long as 800ms to complete.
I would love some help with a more effecient way of doing this. I will be controlling one motor at a time, but while that motor is still moving into position, another command for another motor may (and most likely will) come in.
I have no code yet to handle all the different motors moving at the same time. I could definitely use help with the logic of that. But, for another day... just mentioning it here because that may influence how the above routines are implemented.
Finally, my overall project is to provide a serial servo controller to these 10 DC motors in order to animate a robot head.
Elvis.c (4.27 KB)