       // positionSetpoint(encoder_data);
       break;
   }
-  delay(100);
+  delay(50);
 }
 
 /*********************************************************************************************************

  {
    CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf
    unsigned int canId = CAN.getCanId();

    switch (canId) 
    {        
      case 0x181:
        Serial.println("-----------------------------");
        Serial.print("CORRENTE: ");
        Serial.println(canId, HEX);

            Serial.print("\t");
        }
        Serial.println();
        DataReadState = Position_State;
        }
        break;
      case Position_State:
        if(canId == 0x181)
        {
          Serial.println("-----------------------------");
          Serial.print("POSICAO: ");
          Serial.println(canId, HEX);

          for(int i = 0; i<len; i++)    // print the data
          {
              Serial.print(buf[i], HEX);
              Serial.print("\t");
          }
          Serial.println();
          DataReadState = Amplification_State;
        }
        break;
      case Amplification_State:
        if(canId == 0x321)
        {
          Serial.println("-----------------------------");
          Serial.print("AMPLIFICACAO: ");
          Serial.println(canId, HEX);

          for(int i = 0; i<len; i++)    // print the data
          {
              Serial.print(buf[i], HEX);
              Serial.print("\t");
          }
          Serial.println();
          DataReadState = Current_State;
        }
        break;
      }
  }
