I've created a project where I have a pendulum hanging from a fixed point. The fixed point is a rotary incremental encoder (1024 pulses/revolution). At the bottom of the pendulum I have 2 DC motor fans facing opposite of each other. In order to swing the pendulum into an inverted position (at this time) I have one fan turning on and off using momentum to eventually swing itself up to the inverted position.
My question is, how do I set up the PID controller code to keep the fans working together to balance the fan straight up. With the encoder calibrated at 0 when it is hanging straight down, 512 clicks will get us to the 180 degree straight up point. So I have a variable named PosNum which represents the rotary encoder position.
Code so far:
#include <digitalWriteFast.h>
//ENCODER
// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterrupt 4
#define c_LeftEncoderPinA 19 // Black Wire //Blue and Bare wire to ground
#define c_LeftEncoderPinB 24 // White Wire
#define LeftEncoderIsReversed
volatile bool _LeftEncoderBSet;
volatile long PosNum = 0;
volatile long PosNum1 = 0;
volatile long PosNum2 = 0;
boolean Start = false;
//Pin Assignment for Motors
int dirA1 = 7; //direction of motor A
int dirA2 = 6; // direction of motor A
int speedA = 5; // Throttle of motor A
int dirB1 = 2; //direction of motor B
int dirB2 = 3; // direction of motor B
int speedB = 4; // Throttle of motor B
void setup() {
Serial.begin(115200);
//Motor A Pin Modes
pinMode(dirA1,OUTPUT);
digitalWrite(dirA1,HIGH);
pinMode(dirA2, OUTPUT);
digitalWrite(dirA2, LOW);
pinMode(speedA,OUTPUT);
analogWrite(speedA,0); //0-255 speed pwm
//Motor B Pin Modes
pinMode(dirB1,OUTPUT);
digitalWrite(dirB1,HIGH);
pinMode(dirB2, OUTPUT);
digitalWrite(dirB2, LOW);
pinMode(speedB,OUTPUT);
analogWrite(speedB,0); //0-255 speed pwm
// Left encoder
pinMode(c_LeftEncoderPinA, INPUT); // sets pin A as input
digitalWrite(c_LeftEncoderPinA, LOW); // turn on pullup resistors
pinMode(c_LeftEncoderPinB, INPUT); // sets pin B as input
digitalWrite(c_LeftEncoderPinB, LOW); // turn on pullup resistors
attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
}
int angle = 0;
void loop() {
angle = map(PosNum,-1024,1024,-360,360);
Serial.print(PosNum);
Serial.print("\t");
Serial.print(angle);
Serial.print("\n");
if(Start == false)
{
if(PosNum > PosNum2)
{
analogWrite(speedA,255);
}
else
{
analogWrite(speedA,0) ;
}
}
if(PosNum > 500)
Start = true;
}
// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
// Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
_LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB); // read the input pin
// and adjust counter + if A leads B
#ifdef LeftEncoderIsReversed
PosNum2=PosNum1;
PosNum1=PosNum;
PosNum -= _LeftEncoderBSet ? -1 : +1;
#else
PosNum += _LeftEncoderBSet ? -1 : +1;
#endif
}
I've looked at the arduino PID library and saw the functions. I'm not sure how I would implement these functions to have the 2 fans work together to hold the pendulum upright at the 512 pulse mark coming from the rotary encoder. I hope I provided enough information.
For the PID syntax: for PID()
PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction) *** Would I have 2 Outputs here?
PID(&Input, &Output, &Output2, &Setpoint, Kp, Ki, Kd, Direction) *** Like this?
Where is says Input, I'm assuming thats my PosNum variable from the encoder
My Setpoint would be set to 512.
Output would be speedA
Output2 would be speedB
Kp,Ki,Kd, is there a self tuning feature? Or do i have to hard code these values in via SetTunings()?
thanks for any advice.