Pomiar częstotliwości sygnału na wejściu Arduino.

#include <Arduino.h>
#include <SPI.h>
#include <mcp2515.h>

unsigned long Htime;              //integer for storing high time
unsigned long Ltime;                //integer for storing low time
unsigned long Ttime;            // integer for storing total time of a cycle
unsigned int frequency;        //storing input signal frequency
unsigned int speed;
int rpm = 0;
int rpmScaleFactor = 20;
int tachoFrq = 0; 
int ABSscaleFactor = 25;
int CanId = 0;
char gear = 0;

int sensorPin = 8;
int tachoPin = 3;
int reversePin = 7;

struct can_frame canMsgOut1;
struct can_frame canMsgIn;
MCP2515 mcp2515(10);


void animation();
void frequencyMeasure ();
void tacho();
void CANrecieve();
void debugPrint();

void setup()

{
    pinMode(sensorPin,INPUT);
    pinMode(tachoPin, OUTPUT);
    pinMode(reversePin, OUTPUT);
    Serial.begin(9600);
    mcp2515.reset();
    mcp2515.setBitrate(CAN_100KBPS);
    mcp2515.setNormalMode();
    animation();
}

void loop() {
    frequencyMeasure();
    mcp2515.sendMessage(&canMsgOut1);
    tacho();
    CANrecieve();
  //  debugPrint();

    }

void frequencyMeasure() {
    Htime=pulseInLong(sensorPin,HIGH);      //read high time
    Ltime=pulseInLong(sensorPin,LOW);        //read low time
    Ttime = Htime+Ltime;                    // period
    frequency=1000000/Ttime;    //getting frequency with Ttime is in Micro seconds
    speed = 10*frequency/65;
    unsigned int a = frequency * ABSscaleFactor;
    int b = (a % 2560)/10;
    int c = a / 2560; 
    /*Serial.print(a);
    Serial.print(" | ");
    Serial.print(Ttime);
    Serial.print(" | ");
    Serial.print("Frequency of signal: ");
    Serial.print(frequency);
    Serial.print(" Hz | Speed: ");
    Serial.print(speed);
    Serial.print(" km/h | ");
    Serial.print(c);
    Serial.print(" | ");
    Serial.println(b);
  */
  canMsgOut1.can_id  = 0x208;
  canMsgOut1.can_dlc = 8;
  canMsgOut1.data[0] = 0x00;
  canMsgOut1.data[1] = 0x00;
  canMsgOut1.data[2] = 111;
  canMsgOut1.data[3] = 255;
  canMsgOut1.data[4] = c;
  canMsgOut1.data[5] = b;
  canMsgOut1.data[6] = c;
  canMsgOut1.data[7] = b;
}

void tacho() {
  if (rpm <620)
  {
    noTone(tachoPin);
  }
  else {
tachoFrq= rpm / rpmScaleFactor;  
tone(tachoPin,tachoFrq);  // min frequency possible to achieve with this function is 31Hz!! max is 65,535Hz
}
}

void CANrecieve() {
   
    if (mcp2515.readMessage(&canMsgIn) == MCP2515::ERROR_OK) {
  /*  Serial.print(canMsgIn.can_id, HEX); // print ID
    Serial.print(" "); 
    Serial.print(canMsgIn.can_dlc, HEX); // print DLC
    Serial.print(" ");
      
    for (int i = 0; i<canMsgIn.can_dlc; i++)  {  // print the data
      Serial.print(canMsgIn.data[i],HEX);
      Serial.print(" ");
    
  }
  */
 // Serial.println();  
      }   
      
      if (canMsgIn.can_id == 560) // Gear selector
      {
        switch (canMsgIn.data[0])
        {
        case 5:
        // Serial.println("D");
          gear = 'D';
          digitalWrite(reversePin, LOW);
          break;          
        case 6:
        //  Serial.println("N");
          gear = 'N';
          digitalWrite(reversePin, LOW);
          break;
        case 7:
        //  Serial.println("R");
          gear = 'R';
          digitalWrite(reversePin, HIGH);
          break;
        case 8:
        //  Serial.println("P");
          gear = 'P';
          digitalWrite(reversePin, LOW);
          break;        
        default:
          break;
        }
 
      }
else if (canMsgIn.can_id == 776){  // Tacho
    rpm = canMsgIn.data[1] *256 + canMsgIn.data[2];
    //Serial.print("  |  ");
    //Serial.println(rpm);
}

      else
      {
        // Serial.println("nope");
      }
      
}

void animation() { // Gauge animation function
  tone(tachoPin, 320);
  delay (300);
  tone(tachoPin, 100);
  delay(600);
  noTone(tachoPin);
};

void debugPrint() {
    Serial.print(gear);
    Serial.print(" | ");
    Serial.print(rpm);
    Serial.print(" RPM | ");
    Serial.print(frequency);
    Serial.print(" Hz | ");
    Serial.print(speed);
    Serial.println(" km/h");
}