ReadSpeedEncoderMotor.cpp

float read_speed(int select)
{
    //read velocity of selected motor
    //return velocity in rad/s
    const float PI=3.1415926;
    const int Encoder_1_round = 44; //define number of pulses in one round of encoder
 
     currentEncoder = Encoder_1.read();
     
    float rot_speed;           //rotating speed in rad/s
    const int interval = 1000; //choose interval is 1 second (1000 milliseconds)
    currentMillis = millis();

    if (currentMillis - previousMillis > interval)
    {
        previousMillis = currentMillis;
        rot_speed = (float)((currentEncoder - previousEncoder) * 2 * PI / Encoder_1_round);
        //convert to rpm
        float rpm=rot_speed*60/(2*PI);
        previousEncoder = currentEncoder;
        return rpm;
    }
}

Mới sửa lại