Hello,
First time posting here usually have found my solution by searching but nothing is coming up quite right.
Like the subject says I am trying to control 2 motors (actually will be a hydraulic control coil but motors work for testing purposes) via 2 potentiometers (POT) and the whole system is watching 2 geartooth sensors running through a somewhat PID setup. I have the motors turning on when I turn the POT and have a gear which they spin and the sensor reads the teeth and adjusts the motor speed to match what I have the POT set at. The whole system functions properly when everything is hooked up. The issue I have is that if the sensors are not hooked up and I turn the POT up and back down the motor keeps running, this is very bad for the final use of this setup. If I turn the other POT up after the first was put back to 0 the first motor stops and the second runs and continues when the second POT is turned down, the reverse happens too. I need to be able to shut the motor off via turning the knob all the way down if I loose a connection.
Below is the code I am running and having the issue with.
int driveLa = 8;
int driveLb = 11;
int driveRa = 12;
int driveRb = 13;
int driveLspeed = 9;
int driveRspeed = 10;
int potL = 0;
int potR = 1;
int PWM_valL = 0;
int speed_reqL = 0; // speed (Set Point)
int speed_actL = 0; // speed (actual value)
float KpL = 4; // PID proportional control Gain
float KdL = 1; // PID Derivitave control gain
int PWM_valR = 0;
int speed_reqR = 0; // speed (Set Point)
int speed_actR = 0; // speed (actual value)
float KpR = 4; // PID proportional control Gain
float KdR = 1; // PID Derivitave control gain
volatile byte half_revolutionsL;
unsigned int rpmL;
unsigned long timeoldL;
volatile byte half_revolutionsR;
unsigned int rpmR;
unsigned long timeoldR;
void setup()
{
pinMode(driveLa, OUTPUT);
pinMode(driveLb, OUTPUT);
pinMode(driveRa, OUTPUT);
pinMode(driveRb, OUTPUT);
pinMode(driveLspeed, OUTPUT);
pinMode(driveRspeed, OUTPUT);
digitalWrite(driveLa,LOW);
digitalWrite(driveLb,HIGH);
digitalWrite(driveRa,HIGH);
digitalWrite(driveRb,LOW);
pinMode(potL, INPUT);
pinMode(potR, INPUT);
Serial.begin(9600);
attachInterrupt(0, rpm_funL, CHANGE);
half_revolutionsL = 0;
rpmL = 0;
timeoldL = 0;
attachInterrupt(1, rpm_funR, CHANGE);
half_revolutionsR = 0;
rpmR = 0;
timeoldR = 0;
}
void loop()
{
speed_reqL = analogRead(potL);
{ PWM_valL= updatePid(PWM_valL, speed_reqL, rpmL); // compute PWM value
analogWrite(driveLspeed,PWM_valL);}
if (half_revolutionsL >= 10) {
//Update RPM every 10 counts, increase this for better RPM resolution,
//decrease for faster update
rpmL = 1*1000/(millis() - timeoldL)*half_revolutionsL;
timeoldL = millis();
half_revolutionsL = 0;
//PWM_valL= updatePid(PWM_valL, speed_reqL, rpmL); // compute PWM value
//analogWrite(driveLspeed,PWM_valL);
}
speed_reqR = analogRead(potR);
{ PWM_valR= updatePid(PWM_valR, speed_reqR, rpmR);
analogWrite(driveRspeed,PWM_valR); }
if (half_revolutionsR >= 10) {
//Update RPM every 10 counts, increase this for better RPM resolution,
//decrease for faster update
rpmR = 1*1000/(millis() - timeoldR)*half_revolutionsR;
timeoldR = millis();
half_revolutionsR = 0;
//PWM_valR= updatePid(PWM_valR, speed_reqR, rpmR); // compute PWM value
//analogWrite(driveRspeed,PWM_valR);
}
Serial.print("SP:"); Serial.print(speed_reqL);
Serial.print(" PWM:"); Serial.print(PWM_valL);
Serial.print(" RPM:"); Serial.print(rpmL,DEC);
Serial.print("SP:"); Serial.print(speed_reqR);
Serial.print(" PWM:"); Serial.print(PWM_valR);
Serial.print(" RPM:"); Serial.println(rpmR,DEC);
} // end void loop ()
int updatePid(int commandL, int targetValueL, int currentValueL) { // compute PWM value
float pidTermL = 0; // PID correction
int errorL=0;
static int last_errorL=0;
errorL = abs(targetValueL) - abs(currentValueL);
pidTermL = (KpL * errorL) + (KdL * (errorL - last_errorL));
last_errorL = errorL;
return constrain(commandL + int(pidTermL), 0, 255);
}
int updatePidR(int commandR, int targetValueR, int currentValueR) { // compute PWM value
float pidTermR = 0; // PID correction
int errorR=0;
static int last_errorR=0;
errorR = abs(targetValueR) - abs(currentValueR);
pidTermR = (KpR * errorR) + (KdR * (errorR - last_errorR));
last_errorR = errorR;
return constrain(commandR + int(pidTermR), 0, 255);
}
void rpm_funL()
{
half_revolutionsL++;
//Each rotation, this interrupt function is run twice
}
void rpm_funR()
{
half_revolutionsR++;
//Each rotation, this interrupt function is run twice
}