Controlling an animatronics bust

Using Arduino Project Guidance
54

This code mostly works. It is modified from the DIY-Servo code (unfortunately I forget the author):

int PWMPin = 5;
int PWMPin2 = 6;
int potPin = A0;

float KP = 2; // PID: position multiplier (gain) was 2 Adjust to get more aggressive or conservative control
float KI = 0; // PID: Intergral multiplier (gain) was .05 set about the same as your manual response time Seconds/4
float KD = 1; // PID: Derivative multiplier (gain) http://www.expertune.com/tutor.html

int lastError = 0;
int sumError = 0;

//int iMax = 1;
//int iMin = 0;

int setPos = 0;
int dutyMax = 254;

void setup() {

pinMode(PWMPin, OUTPUT);
pinMode(PWMPin2, OUTPUT);
setPwmFrequency(PWMPin, 255);
Serial.begin(9600);

}

void loop() {

int val = analogRead(potPin);
val = map(val,0,1023,0,255);

checkInput();

int error = val - setPos;

int ms = KP * error + KD * (error - lastError) + KI * (sumError);

lastError = error;
sumError += error;

// if(sumError > iMax){
// sumError = iMax;
// }
// else if(sumError < iMin){
// sumError = iMin;
// }

// BACKWARD DIRECTION
if(ms > 0){
int motorSpeed = ms;
if(motorSpeed > dutyMax){
motorSpeed = dutyMax;
}
PWMPin = 5;
PWMPin2 = 6;

analogWrite(PWMPin2, 0);
analogWrite(PWMPin, motorSpeed);
}

// FORWARD DIRECTION
if(ms < 0){
ms = -1 * ms;
int motorSpeed = ms;
if(motorSpeed > dutyMax){
motorSpeed = dutyMax;
}

PWMPin = 6;
PWMPin2 = 5;

analogWrite(PWMPin2, 0);
analogWrite(PWMPin, motorSpeed);
}

}

void checkInput(){
if(Serial.available()){
byte ch = Serial.read();

if (ch == '+'){
setPos = setPos+5;
}
if (ch == '-'){
setPos = setPos-5;
}
if(ch == 's'){
analogWrite(PWMPin,0);

}

}

setPos=constrain(setPos,0,255);

}

void setPwmFrequency(int pin, int divisor) {
byte mode;
if(pin == 5 || pin == 6 || pin == 9 || pin == 10) { // Timer0 or Timer1
switch(divisor) {
case 1:
mode = 0x01;
break;
case 8:
mode = 0x02;
break;
case 64:
mode = 0x03;
break;
case 256:
mode = 0x04;
break;
case 1024:
mode = 0x05;
break;
default:
return;
}
}
else if(pin == 3 || pin == 11) {
switch(divisor) {
case 1:
mode = 0x01;
break;
case 8:
mode = 0x02;
break;
case 32:
mode = 0x03;
break;
case 64:
mode = 0x04;
break;
case 128:
mode = 0x05;
break;
case 256:
mode = 0x06;
break;
case 1024:
mode = 0x7;
break;
default:
return;
}
TCCR2B = TCCR2B & 0b11111000 | mode; // Timer2
}
}

But it makes a ton of noise, even at idle. And the difference between setpos and currpos needs to be very large before the motor will even move. However, it will fairly accurately move to that position and "stop." Well, I say stop, but more like it sits there and whines. It does undershoot a little, but I think that is because it is setting the pwm so low that the motor stops turning and can't make it that last bit.