SMS_STS with ST3215-HS and ESP8266

[weingaunity]

Hi, i try to get a ST3215-HW working with an ESP8266.
The motor can be controlled. But reading back Values yield to e.g. "read Voltage err".

Do you have any hints how to solve this issue?

#include "SCServo.h"
#include <SoftwareSerial.h>

SMS_STS serialservo;

SoftwareSerial Debug =  SoftwareSerial(3, 1);


/* Simple driver Schematic
   Connected to Lolin WEMOS D1 R2 & mini
      3V3 ----+
             +++
             | | 10k
             +++
              |
     RXD2  ----+---------------BUS
              |          9V---VCC
            +++++        GND--GND
             \ /
            --+--
              |
     TXD2 -----+  
*/


void scanMotors()
{
  int Pos;
  int Speed;
  int Load;
  int Voltage;
  int Temper;
  int Move;
  int Current;
  if(serialservo.FeedBack(1)!=-1){
    Pos = serialservo.ReadPos(-1);
    Speed = serialservo.ReadSpeed(-1);
    Load = serialservo.ReadLoad(-1);
    Voltage = serialservo.ReadVoltage(-1);
    Temper = serialservo.ReadTemper(-1);
    Move = serialservo.ReadMove(-1);
    Current = serialservo.ReadCurrent(-1);
    Debug.print("Position:");
    Debug.println(Pos);
    Debug.print("Speed:");
    Debug.println(Speed);
    Debug.print("Load:");
    Debug.println(Load);
    Debug.print("Voltage:");
    Debug.println(Voltage);
    Debug.print("Temper:");
    Debug.println(Temper);
    Debug.print("Move:");
    Debug.println(Move);
    Debug.print("Current:");
    Debug.println(Current);
    delay(10);
  }else{
    Debug.println("FeedBack err");
    delay(500);
  }

}

void setup()
{
  Serial.begin(1000000,SERIAL_8N1);
  Serial.swap();
  serialservo.pSerial = &Serial;
  delay(100);
  
  Debug.begin(9600);
  Debug.println("Init");
  delay(1000);
  //scanMotors();
}

void loop()
{
  int Voltage = serialservo.ReadVoltage(1);
  if(Voltage!=-1){
	Debug.print("Servo Voltage:");
    Debug.println(Voltage, DEC);
    delay(10);
  }else{
    Debug.println("read Voltage err");
    delay(500);
  }
  //serialservo.EnableTorque(1,false);
  serialservo.WritePosEx(1, 4095, 3400, 50);//servo(ID1) speed=3400，acc=50，move to position=4095.
  delay(2000);
  
  //serialservo.EnableTorque(1,true);
  serialservo.WritePosEx(1, 2000, 1500, 50);//servo(ID1) speed=3400，acc=50，move to position=2000.
  delay(2000);
}

Thx.

[weingaunity]

I think, i got it working:

i changed the writeSCS in that way, that it waits until the "ECHO" byte is received. So the buffer is empty if the servo sends his package.

int SCSerial::writeSCS(unsigned char *nDat, int nLen)
{
	if(nDat==NULL){
		return 0;
	}
  for(int i=0;i<nLen;i++)
  {
    pSerial->write(nDat[i]);
    int c=0;
    while(!pSerial->available())
    {

    }
    if (nDat[i]!=pSerial->read())
    {
      return -1;
    }
  }
	//return pSerial->write(nDat, nLen);
  return nLen;
}