Hello All
I am trying to use dc42's code to measure a low frequency speed from the flywheel of an internal combustion engine. The signal of interest is a 5V square wave in the order of 30 to 40Hz. However, using dc40's code there are rogue high values (50 to 60Hz) appearing which is completely throwing my speed control system.
I am using a servo motor to control the throttle angle based on speed to keep the speed at a setpoint, using a PID library. Would this be throwing the ISR timer? Can the two work together? Where are these rogue values coming from? They appear randomly, with every 20 to 40 seconds of running:
#include <PID_v1.h> //include PID library in sketch
#include <Wire.h>
#include <Servo.h>
#define ECU2_RESET 8 //was 3
#define SERVO_PIN 9 //was 5
#define OVERSPEED_PIN 10 //overspeed to master
#define RELAY_PIN 11 //local overspeed relay
volatile unsigned long firstPulseTime;
volatile unsigned long lastPulseTime;
volatile unsigned long numPulses;
unsigned long over_speed_setpoint = 50; //define an over speed setpoint (i.e 3000rpm) here in correct units = 3000rpm
double speed_setpoint = 41 ; // see engine calcs overview for this calc. or onenote To Do sheet. 48000=2500rpm, 50000=2400rpm
unsigned int sample_time = 100;
unsigned long num_iterations;
/////////////////////PID SETTINGS///////////////////////////////////////
double Kp = 1.0; //was o.008
double Ki = 0.7; // was 0.0022
double Kd=0.0;
////////////////////SERVO SETTINGS//////////////////////////////////////////////////
double throttle_setpoint; // variable to store the desired throttle angle - WHAT SHOULD THIS BE INITIALISED AT??
int throttle_sig; //whats set to the servo = (90-throttle_setpoint) calculated by map() function
Servo myservo; //initalising servo as servo object
double freq_1=0; //variable for frequency readout
boolean engine_running = false; // this is false until the engine starter motor is engaged
PID myPID(&freq_1, &throttle_setpoint, &speed_setpoint, Kp, Ki, Kd, DIRECT); //PID setup using speed avg //REVERSE
void isr()
{
unsigned long now = micros();
if (numPulses == 1)
{
firstPulseTime = now;
}
else
{
lastPulseTime = now;
}
++numPulses;
}
void setup()
{
Serial.begin(9600);
Serial.println("Setup Finished");
Serial.print("This run we have : ");
Serial.print("Kp = ");
Serial.print(Kp);
Serial.print(" Ki = ");
Serial.print(Ki);
Serial.print(" Kd = ");
Serial.print(Kd);
Serial.print("Sample time = ");
Serial.print(sample_time);
Serial.print("Speed setpoint = ");
Serial.println(speed_setpoint);
pinMode(21, INPUT); //input for the HES signal
digitalWrite(21,HIGH); // added to prevent floating values???
pinMode(OVERSPEED_PIN, OUTPUT); //pin 4 will be HIGH when engine speed is too high and shutdown is required, LOW when ok to keep running.OVERSPEED PIN
pinMode(RELAY_PIN,OUTPUT); //pin 7 as local overspeed pin
digitalWrite(OVERSPEED_PIN,LOW); //initailly set this to low to say that there is no overspeed on system intialisation
digitalWrite(RELAY_PIN,HIGH); //initailly set this to low to say that there is no overspeed on system intialisation
myservo.attach(SERVO_PIN);//attaching the servo to PIN 9.
myservo.write(8);// setting inital value of 85 to get it started on idle
myPID.SetMode(AUTOMATIC); //this sets the PID algoithm to automatic, i.e. it will compute every loop
}
// Measure the frequency over the specified sample time in milliseconds, returning the frequency in Hz
float readFrequency(unsigned int sampleTime)
{
numPulses = 0; // prime the system to start a new reading
attachInterrupt(2, isr, RISING); // enable the interrupt
delay(sampleTime);
detachInterrupt(2);
return (numPulses < 3) ? 0 : (1000000.0 * (float)(numPulses - 2))/(float)(lastPulseTime - firstPulseTime); // if the number of pulses is less than 3, then 0, otherwise -
}
void loop()
{
if(engine_running==true){
num_iterations++;
}
freq_1 = readFrequency(sample_time);
if(freq_1 > 5 && engine_running == false && freq_1 < 1000){ //if the engine is cranking then the engine_running variable is set to on so the PID controller is activated
Serial.println("Engine is now running, ECU activated");
engine_running = true;
}
if(engine_running==true ){ //if closed loop control is need then the master controller will set pin 3 to HIGH AND the engine has started and the servo should be activated
myPID.Compute();//required for PID to work
throttle_sig=(int)map(throttle_setpoint, 0, 255, 6, 87);// remapping the PID controller output (0-255), set by the PID setoutput function in the setup to 90 to 0 i.e. 90 degrees throttle_setpoint is actually 0 degrees throttle angle ---23.05.2016 was 0,255,0,90 changed to 87 as was conking on start
myservo.write(throttle_sig); // this is writing the mapped PID output to the servo
// Serial.print("The PID OUTPUT is: ");
Serial.print(throttle_setpoint);
Serial.print("\t");
// Serial.print("The servo sig is: ");
Serial.print(throttle_sig);
Serial.print("\t");
// Serial.print("The speed is is: ");
Serial.print(freq_1);
Serial.print("\t");
Serial.print(freq_2);
Serial.print("\t");
Serial.println(millis());
}
if(freq_1 >= over_speed_setpoint){
Serial.println(freq_1);
digitalWrite(OVERSPEED_PIN,HIGH); //sending 5v signal to master to indicate overspeed
digitalWrite(RELAY_PIN,LOW);
Serial.println("OVERSPEED");
}
else {
digitalWrite(OVERSPEED_PIN,LOW);//telling master that there is no overspeed present
digitalWrite(RELAY_PIN,HIGH); //keeping local overspeed relay switched on as there is no issues
}
}