Could someone give idea why my data reading is slow when I run the code?

Projects Programming
1

Hello guys what I am doing now is using a lidar sensor that is attached to the servo motor so that it scans rotationally. Without the controlling part, the codes work well but when I try to run with the controlling part not only the servo but also the lidar data gathers really slow. Could someone give an idea of why it happens?
The control part means the fuzzy logic inference system. I used the Matlab FIS and transformed it into the c code from makeproto.com.
(using arduino Leonardo)
I am a newbie so that sorry if uploading with below codes is not permitted



#include <SoftwareSerial.h>  //header file of software serial port
#include <Servo.h>
#include "fis_header.h"

Servo myservo; // create servo object to control a servo, max = 8 servos
int myservo_pin = 6; // pin that controls the servo
int myservo_speed = 8; // how fast the servo moves, 1 = very fast, 10 = very slow
long myservo_movetime = 0; // next time in millis servo next moves
int myservo_gPos = 0; // target position to move towards
int myservo_cPos = 0; // current postion of servo

int pos = 0; // variable to store the servo position
int cPos = 0; // current position
int gPos; // goal position
int smoother = 30; // delay between moves, gives appearance of smooth motion

SoftwareSerial mySerial(8,9); //define software serial port name as Serial1 and define pin2 as RX and pin3 as TX

/* For Arduinoboards with multiple serial ports like DUEboard, interpret above two pieces of code and directly use Serial1 serial port*/

float dist;  //actual distance measurements of LiDAR
int strength; //signal strength of LiDAR
int check;  //save check value
int i;
int uart[9];  //save data measured by LiDAR
const int HEADER=0x59;  //frame header of data package


// Number of inputs to the fuzzy inference system
const int fis_gcI = 2;
// Number of outputs to the fuzzy inference system
const int fis_gcO = 1;
// Number of rules to the fuzzy inference system
const int fis_gcR = 21;

FIS_TYPE g_fisInput[fis_gcI];
FIS_TYPE g_fisOutput[fis_gcO];

// Setup routine runs once when you press reset:
void setup()
{

    //Serial.begin(9600);
    // initialize the Analog pins for input.
    // Pin mode for Input: Distance
    //pinMode(0 , INPUT);
    // Pin mode for Input: Angle
    //pinMode(1 , INPUT);


    // initialize the Analog pins for output.
    // Pin mode for Output: Output_power
    //pinMode(2 , OUTPUT);

  mySerial.begin(115200);  //set bit rate of serial port connecting LiDAR with Arduino
  //Serial.begin(115200);
  myservo.attach(6); 

}
void lidar(){

  
  if (mySerial.available()) {  //check if serial port has data input
    if(mySerial.read() == HEADER) {  //assess data package frame header 0x59
      uart[0]=HEADER;
      if (mySerial.read() == HEADER) { //assess data package frame header 0x59
        uart[1] = HEADER;
        for (i = 2; i < 9; i++) { //save data in array
          uart[i] = mySerial.read();
        }
        check = uart[0] + uart[1] + uart[2] + uart[3] + uart[4] + uart[5] + uart[6] + uart[7];
        if (uart[8] == (check & 0xff)){ //verify the received data as per protocol
          dist = (uart[2] + uart[3] * 256)* 0.01;     //calculate distance value
          //strength = uart[4] + uart[5] * 256; //calculate signal strength value
          Serial.print("dist(meter) = ");
          Serial.print(dist); //output measure distance value of LiDAR
          Serial.print('\t');
          //Serial.print("strength = ");
          //Serial.print(strength); //output signal strength value
          //Serial.print('\n');
        }
      }
    }
  }
}



void moveServo() {
if (cPos < gPos) myservo.write(cPos+1);
if (cPos > gPos) myservo.write(cPos-1);
//if (cPos == gPos) // nothing
myservo_movetime = millis() + smoother;
}


//***********************************************************************
// Support functions for Fuzzy Inference System                          
//***********************************************************************
// Trapezoidal Member Function
FIS_TYPE fis_trapmf(FIS_TYPE x, FIS_TYPE* p)
{
    FIS_TYPE a = p[0], b = p[1], c = p[2], d = p[3];
    FIS_TYPE t1 = ((x <= c) ? 1 : ((d < x) ? 0 : ((c != d) ? ((d - x) / (d - c)) : 0)));
    FIS_TYPE t2 = ((b <= x) ? 1 : ((x < a) ? 0 : ((a != b) ? ((x - a) / (b - a)) : 0)));
    return (FIS_TYPE) min(t1, t2);
}

// Triangular Member Function
FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
{
    FIS_TYPE a = p[0], b = p[1], c = p[2];
    FIS_TYPE t1 = (x - a) / (b - a);
    FIS_TYPE t2 = (c - x) / (c - b);
    if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
    if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
    if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
    t1 = min(t1, t2);
    return (FIS_TYPE) max(t1, 0);
}

FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
{
    return min(a, b);
}

FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
{
    return max(a, b);
}

FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
{
    int i;
    FIS_TYPE ret = 0;

    if (size == 0) return ret;
    if (size == 1) return array[0];

    ret = array[0];
    for (i = 1; i < size; i++)
    {
        ret = (*pfnOp)(ret, array[i]);
    }

    return ret;
}


//***********************************************************************
// Data for Fuzzy Inference System                                       
//***********************************************************************
// Pointers to the implementations of member functions
_FIS_MF fis_gMF[] =
{
    fis_trapmf, fis_trimf
};

// Count of member function for each Input
int fis_gIMFCount[] = { 7, 3 };

// Count of member function for each Output 
int fis_gOMFCount[] = { 5 };

// Coefficients for the Input Member Functions
FIS_TYPE fis_gMFI0Coeff1[] = { 0, 0, 1, 2 };
FIS_TYPE fis_gMFI0Coeff2[] = { 1, 2, 3 };
FIS_TYPE fis_gMFI0Coeff3[] = { 2, 3, 4 };
FIS_TYPE fis_gMFI0Coeff4[] = { 3, 4, 5 };
FIS_TYPE fis_gMFI0Coeff5[] = { 4, 5, 6 };
FIS_TYPE fis_gMFI0Coeff6[] = { 5, 6, 7 };
FIS_TYPE fis_gMFI0Coeff7[] = { 6, 7, 12, 12 };
FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3, fis_gMFI0Coeff4, fis_gMFI0Coeff5, fis_gMFI0Coeff6, fis_gMFI0Coeff7 };
FIS_TYPE fis_gMFI1Coeff1[] = { 60, 60, 80, 90 };
FIS_TYPE fis_gMFI1Coeff2[] = { 80, 90, 100};
FIS_TYPE fis_gMFI1Coeff3[] = { 90, 100, 120, 120};
FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3 };
FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff };

// Coefficients for the Output Member Functions
FIS_TYPE fis_gMFO0Coeff1[] = { 0, 0, 45, 65 };
FIS_TYPE fis_gMFO0Coeff2[] = { 55, 75, 95 };
FIS_TYPE fis_gMFO0Coeff3[] = { 85, 105, 125 };
FIS_TYPE fis_gMFO0Coeff4[] = { 115, 135, 155 };
FIS_TYPE fis_gMFO0Coeff5[] = { 140, 155, 180, 180 };
FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };

// Input membership function set
int fis_gMFI0[] = { 0, 1, 1, 1, 1, 1, 0 };
int fis_gMFI1[] = { 0, 1, 0 };
int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1};

// Output membership function set
int fis_gMFO0[] = { 0, 1, 1, 1, 0 };
int* fis_gMFO[] = { fis_gMFO0};

// Rule Weights
FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };

// Rule Type
int fis_gRType[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };

// Rule Inputs
int fis_gRI0[] = { 1, 1 };
int fis_gRI1[] = { 1, 2 };
int fis_gRI2[] = { 1, 3 };
int fis_gRI3[] = { 2, 1 };
int fis_gRI4[] = { 2, 2 };
int fis_gRI5[] = { 2, 3 };
int fis_gRI6[] = { 3, 1 };
int fis_gRI7[] = { 3, 2 };
int fis_gRI8[] = { 3, 3 };
int fis_gRI9[] = { 4, 1 };
int fis_gRI10[] = { 4, 2 };
int fis_gRI11[] = { 4, 3 };
int fis_gRI12[] = { 5, 1 };
int fis_gRI13[] = { 5, 2 };
int fis_gRI14[] = { 5, 3 };
int fis_gRI15[] = { 6, 1 };
int fis_gRI16[] = { 6, 2 };
int fis_gRI17[] = { 6, 3 };
int fis_gRI18[] = { 7, 1 };
int fis_gRI19[] = { 7, 2 };
int fis_gRI20[] = { 7, 3 };
int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4, fis_gRI5, fis_gRI6, fis_gRI7, fis_gRI8, fis_gRI9, fis_gRI10, fis_gRI11, fis_gRI12, fis_gRI13, fis_gRI14, fis_gRI15, fis_gRI16, fis_gRI17, fis_gRI18, fis_gRI19, fis_gRI20 };

// Rule Outputs
int fis_gRO0[] = { 1 };
int fis_gRO1[] = { 1 };
int fis_gRO2[] = { 1 };
int fis_gRO3[] = { 1 };
int fis_gRO4[] = { 2 };
int fis_gRO5[] = { 1 };
int fis_gRO6[] = { 2 };
int fis_gRO7[] = { 3 };
int fis_gRO8[] = { 2 };
int fis_gRO9[] = { 2 };
int fis_gRO10[] = { 1 };
int fis_gRO11[] = { 2 };
int fis_gRO12[] = { 4 };
int fis_gRO13[] = { 3 };
int fis_gRO14[] = { 4 };
int fis_gRO15[] = { 5 };
int fis_gRO16[] = { 4 };
int fis_gRO17[] = { 5 };
int fis_gRO18[] = { 5 };
int fis_gRO19[] = { 5 };
int fis_gRO20[] = { 5 };
int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4, fis_gRO5, fis_gRO6, fis_gRO7, fis_gRO8, fis_gRO9, fis_gRO10, fis_gRO11, fis_gRO12, fis_gRO13, fis_gRO14, fis_gRO15, fis_gRO16, fis_gRO17, fis_gRO18, fis_gRO19, fis_gRO20 };

// Input range Min
FIS_TYPE fis_gIMin[] = { 0, 60 };

// Input range Max
FIS_TYPE fis_gIMax[] = { 12, 15 };

// Output range Min
FIS_TYPE fis_gOMin[] = { 0 };

// Output range Max
FIS_TYPE fis_gOMax[] = { 180 };

//***********************************************************************
// Data dependent support functions for Fuzzy Inference System           
//***********************************************************************
FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
{
    FIS_TYPE mfOut;
    int r;

    for (r = 0; r < fis_gcR; ++r)
    {
        int index = fis_gRO[r][o];
        if (index > 0)
        {
            index = index - 1;
            mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
        }
        else if (index < 0)
        {
            index = -index - 1;
            mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
        }
        else
        {
            mfOut = 0;
        }

        fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
    }
    return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
}

FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
{
    FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
    FIS_TYPE area = 0;
    FIS_TYPE momentum = 0;
    FIS_TYPE dist, slice;
    int i;

    // calculate the area under the curve formed by the MF outputs
    for (i = 0; i < FIS_RESOLUSION; ++i){
        dist = fis_gOMin[o] + (step * i);
        slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
        area += slice;
        momentum += slice*dist;
    }

    return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
}

//***********************************************************************
// Fuzzy Inference System                                                
//***********************************************************************
void fis_evaluate()
{
    FIS_TYPE fuzzyInput0[] = { 0, 0, 0, 0, 0, 0, 0 };
    FIS_TYPE fuzzyInput1[] = { 0, 0, 0 };
    FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, };
    FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
    FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
    FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
    FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
    FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
    FIS_TYPE sW = 0;

    // Transforming input to fuzzy Input
    int i, j, r, o;
    for (i = 0; i < fis_gcI; ++i)
    {
        for (j = 0; j < fis_gIMFCount[i]; ++j)
        {
            fuzzyInput[i][j] =
                (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
        }
    }

    int index = 0;
    for (r = 0; r < fis_gcR; ++r)
    {
        if (fis_gRType[r] == 1)
        {
            fuzzyFires[r] = FIS_MAX;
            for (i = 0; i < fis_gcI; ++i)
            {
                index = fis_gRI[r][i];
                if (index > 0)
                    fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
                else if (index < 0)
                    fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
                else
                    fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
            }
        }
        else
        {
            fuzzyFires[r] = FIS_MIN;
            for (i = 0; i < fis_gcI; ++i)
            {
                index = fis_gRI[r][i];
                if (index > 0)
                    fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
                else if (index < 0)
                    fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
                else
                    fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
            }
        }

        fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
        sW += fuzzyFires[r];
    }

    if (sW == 0)
    {
        for (o = 0; o < fis_gcO; ++o)
        {
            g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
        }
    }
    else
    {
        for (o = 0; o < fis_gcO; ++o)
        {
            g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
        }
    }
}
// Loop routine runs over and over again forever:
void loop()
{
  long t = millis();
  static long lastT = 0;
  long dt = t - lastT;
  
  lastT = t;

  lidar();
  //servomove(t, dt);
   cPos = myservo.read();
  //Serial.println(myservo.read());
  if (cPos == 60) gPos = 120;
  if (cPos == 120) gPos = 60;
  if (cPos != gPos && t >= myservo_movetime) {
  moveServo();
  }
 

 
 
  // Read Input: Distance
    g_fisInput[0] = 0.5; //analogRead(0);
    // Read Input: Angle
    g_fisInput[1] = 90; //analogRead(1);

    g_fisOutput[0] = 0;

    fis_evaluate();
    //Serial.println(g_fisOutput[0]);
    // Set output vlaue: Output_power
   // analogWrite(2 , g_fisOutput[0]);

}
2

What do you mean exactly by "the controlling part" ??

Which Arduino are you running on? Perhaps if its an 8-bit Arduino like the Uno its simply too slow to run this code - the Uno is about 3 to 4 orders of magnitude slower than a computer - perhaps you don't realize this? That inference stuff looks non-trivial with multiply nested loops.

3

Hello, thank you for the reply. The controlling part means the codes about the FIS and I use Arduino Leonardo.

4

The Leonardo is another 8-bit 16MHz microcontroller like the Uno - I suspect its too slow for your application.

However I'm not sure whether FIS_TYPE is integer or float - if its float that will be even slower as there's no hardware float support in the Leonardo.

If you need some serious processing power you may need a faster Arduino board.

5

Really thank you for your help! I will try to run it with other board

6

The way you are reading serial data WILL prove unreliable. You wait for Serial.available() to return non-zero, then blindly read 9 characters. You should
NEVER call Serial.read() without ensuring there is actually data there to read. This means either checking Serial.available() before EACH call to Serial.read(), or waiting until Serial.available() shows ALL the characters you want to read are available before reading any of them.

7

Hello sir, thank you for the feedback however, I am quite a newbie so can't get your point well. Could you explain more about "This means either checking Serial.available() before EACH call to Serial.read()" in detail?
If I use the lidar() and servo() without the other part of codes, it gives me distance data well. So you mean lidar() and servo() structure should be modified to keep working with other parts?
So sorry to ask a lot

8

Don't call read() unless you know enough characters are available or else you will read junk values.

You might want to consider a blocking version of read():

int blocking_read()
{
  while (Serial.available() == 0) {}  // busy-wait for character
  return Serial.read() ;
}
9

ohhh I see thanks I should try that! thanks!!

10

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.