Sorry about that, below is my code.
/*
Project:
Name: Daniel G
Date: 10/15/2012
Company:
*/
/*
EnA | In1 | In2 | Description
0 | X | X | Free Running Motor Stop
1 | 1 | 0 | Forward
1 | 0 | 1 | ReverslaveEncoder
1 | 1 | 1 | Fast Motor Stop
CHA: ____EEEE____EEEE____EEEE____EEEE
CHB: EE____EEEE____EEEE____EEEE____EE
When CHA rislaveEncoders check the state of CHB. From here you can determine the
direction of the encoder corresponding to the Master or Slave
*/
#include <stdio.h>
int In1 = 8; // Red
int In2 = 9; // Orange
int EnA = 10; // Yellow
int encoderPower = 7; // Red
int Master_CHA = 0; // Green - Interrupt INT0 on Pin 2
int Master_CHB = 4; // White
int Slave_CHA = 1; // Purple - Interrupt INT1 on Pin 3
int Slave_CHB = 6; // Teal
int LOOPCOUNTER = 0;
int Master_CHB_val = 0;
int Slave_CHB_val = 0;
volatile float masterEncoder = 0;
volatile float slaveEncoder = 0;
// Control Signal and Error
float u = 0;
float err = 0;
// PID Control
float Kp = 8;
float Ki = .002;
float Kd = .2;
// Error Variables
float err_old = 0;
float err_intgrl = 0;
float err_dot = 0;
int max_int = 200;
int max_der = 200;
void setup()
{
// Serial.begin(9600); // Begin serial communication
// slaveEncodertting Motor Pins
pinMode(In1, OUTPUT); // Setting In1 (direction indicator)
pinMode(In2, OUTPUT); // Setting In2 (direction indicator)
pinMode(EnA, OUTPUT); // Setting PWM output
// Temperary HIGH to encoderPower Pin to give Vcc
pinMode(encoderPower, OUTPUT);
digitalWrite(encoderPower, HIGH);
// Set Encoder Pins & Interrupt
pinMode(Master_CHB, INPUT);
pinMode(Slave_CHB, INPUT);
attachInterrupt(Master_CHA, Master_checkDirection, RISING); // CHA causes interrupt service routine
attachInterrupt(Slave_CHA, Slave_checkDirection, RISING); // CHA causes interrupt service routine
TCCR1B = TCCR1B & 0b11111000 | 0x02; // PWM frequency changed from 500Hz to 3.922KHz - L298 typically can support 25KHz.
}
// ISR for Master (Controller)
void Master_checkDirection()
{
Master_CHB_val = digitalRead(Master_CHB); // When CHA RAISES check CHB
if (Master_CHB_val == 0) masterEncoder += 1.4; // Increment Master count for a direction
else masterEncoder -= 1.4; // Decrement Master count for the other direction
}
// ISR for Slave (Motor)
void Slave_checkDirection()
{
Slave_CHB_val = digitalRead(Slave_CHB); // When CHA RAISES check CHB
if (Slave_CHB_val == 0) slaveEncoder += 0.6; // Increment Slave count for a direction
else slaveEncoder -= 0.6; // Decrement Slave count for the other direction
}
// Function to Set Direction of PWM
void setDirection(boolean In1_State, boolean In2_State)
{
digitalWrite(In1, In1_State);
digitalWrite(In2, In2_State);
}
void loop()
{
err = slaveEncoder - masterEncoder;
// LOOPCOUNTER++;
if (err < 0) setDirection(LOW, HIGH);
else if (err > 0) setDirection(HIGH, LOW);
err_dot = err - err_old;
if (err_dot > max_der) err_dot = max_der;
else if (err_dot < -max_der) err_dot = -max_der;
err_intgrl = err + err_intgrl;
if (err_intgrl > max_int) err_intgrl = max_int;
else if (err_intgrl < -max_int) err_intgrl = -max_int;
u = (float)(abs(Kperr + Kderr_dot + Ki*err_intgrl));
constrain(u, 0, 255);
analogWrite(EnA, u); // Write PWM value as the encoder count of the Master (controller)
// if (LOOPCOUNTER == 1200)
// {
// Serial.print(masterEncoder);
// Serial.print(" || ");
// Serial.print(slaveEncoder);
// Serial.print(" || ");
// Serial.println(err);
// LOOPCOUNTER = 0;
// }
err_old = err;
}