Sending Code to Stepper Motors

Hey!
So I am writing code to send Yaw, Pitch, and Roll angles to three separate stepper motors. The data is constantly updating based on the movement of my phone, so I am constantly reading a range of angles into the serial monitor in the format "P-xx.xx\Yxx.xx\R-xx.xx\P-xx.xx\Yxx.xx\R-xx.xx..."
I currently have the code to convert the data I receive to the appropriate steps and I can read the data into the serial monitor.
My current dilemma is how to pull this data from the serial monitor and read as the three separate inputs which I can turn into separate commands?

This is the code that reads the angles from the phone to the serial monitor

#include <SoftwareSerial.h>
SoftwareSerial mySerial(2, 3);
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
mySerial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly:
if (mySerial.available() > 0){
  Serial.write(mySerial.read());
  }
}

This is the data in the serial monitor ----- P:Pitch; Y:Yaw; R:Roll

Please don't post images of the serial monitor, just copy the text from the serial monitor and paste it into your post between code tags.

Much easier with Serial.parseFloat()

I thought you said you were sending it to serial monitor, not pulling it from serial monitor. Please clarify!

So, I can read it into the serial monitor, but it is a string and the string contains three separate values (Roll, Pitch, and Yaw). I do not know how to separate those values from each other and convert them to an integer.

Do you have control of the way that the data is sent? If so, you can make up packets that are easier to receive reliably and parse. See the serial input basics tutorial.

This code is one of those really bad short example-codes that teaches nothing.
Serial.read() takes a character out of the buffer and if you just print this character
it is printed one time and then gone.

It is better to store the character in a variable.
The Serial-object offers a lot of methods to do more than just read a single character

Look up the reference to the serial-object.
Write some small and extra test-codes to learn how each function behaves.

You can invest your time in two ways:

not recommended

  • trying to be fast fiddling around until it works somehow in the end you know almost nothing

recommended because you really learn.

  • writing small test-programs and to study the different functions
  • storing each test-code with a self-explaining name for future reference

For the small test-programs don't use your smartphone. Use a second microcontroller if you have
Send a constant charactersequence. By doing this it easier to understand what your program does.

It is up to you to decide: using the same amount of time fiddling around with almost no learning or using the same amount of time for learning and become a professional about serial communication

best regards Stefan
https://www.arduino.cc/reference/en/language/functions/communication/serial/parsefloat/

Still confusing! Your sketch can write/print to the serial monitor and the text will appear there, on your PC/laptop for you read. Your sketch can read from the serial monitor if you type some text into the line at the top of the serial monitor and click "send".

What did you mean by "read it into"? Did you mean "read it from" or "write it into"?

I am trying to understand the flow of data here. If I have understood, the source of the data is the accelerometer or gyroscope sensors in your smartphone? You have an app running on the phone which reads those sensors and formats the data in the format you described, then sends that to an Arduino, I guess that might be through Bluetooth? Am I right so far?

The Arduino then reads the data from a Bluetooth receiver and echoes it to your PC where it appears on the serial monitor.

You then want something, maybe a second Arduino, maybe the same Arduino, to read something from the serial monitor and use that to control some stepper motors?

I wonder if your idea of what the serial monitor is for and what role it would take in your project has made you confused? Maybe you just want your Arduino to read the data from the phone and control the stepper motors? Is that right?

If so, perhaps there is no need for serial monitor to be used at all. It might be helpful to use it for monitoring/debugging the sketch, which is it's true purpose, but it would be playing no important part in the process at all. If the PC was not connected, and serial monitor could not be used, you would still want to control the motors?

I think this will do what you want the SafeStringReader from using my SafeString library

//https://forum.arduino.cc/t/sending-code-to-stepper-motors/887236
// Bluetooth

// download and install the SafeString library from
// www.forward.com.au/pfod/ArduinoProgramming/SafeString/index.html
#include "SafeString.h"
#include "SafeStringStream.h"
#include "SafeStringReader.h"

#include <SoftwareSerial.h>
SoftwareSerial mySerial(2, 3);

#define TEST_DATA

#ifdef TEST_DATA
// P-xx.xx\Yxx.xx\R-xx.xx\P-xx.xx\Yxx.xx\R-xx.xx...
// e.g. P-101.33\Y33.55\R-34.55
const char testData[] = "P-101.33\\Y33.55\\R-34.55\\P-201.33\\Y133.55\\R-234.55\\P-1.33\\Y-3.55\\R-4.55\\P21.33\\Y53.55\\R4.55\\";
// note the delimiting \ at the end
createSafeString(sfTestData, 100); // test data in a safeString
cSF(sfRxBuffer,64); // create a SafeString to use for an Rx buffer for sfStream, default rx buffer is only 8 bytes
SafeStringStream sfStream(sfTestData,sfRxBuffer); // set a 64byte RX buffer
#define dataSerial sfStream
#else
#define dataSerial mySerial
#endif


// create an sfReader instance of SafeStringReader class
// that will handle commands upto 11 chars long
// delimited by \ or CarrageReturn or NewLine
// the createSafeStringReader( ) macro creates both the SafeStringReader (sfReader) and the necessary SafeString that holds input chars until a delimiter is found
// args are (ReaderInstanceName, expectedMaxCmdLength, delimiters)
createSafeStringReader(sfReader, 12, "\\\r\n"); // allow for 11 char + delimiter

float phoneYaw, phonePitch, phoneRoll;
float currentPYR [3], DCM [3][3], EulerPYR [3], rotPYR [3], stepPYR [3];

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200); // make this fast
  for (int i = 10; i > 0; i--) { // pause a little to give you time to open the Arduino monitor
    Serial.print(i); Serial.print(' '); delay(500);
  }
  Serial.println();
  SafeString::setOutput(Serial); // enable error messages and SafeString.debug() output to be sent to Serial
#ifdef TEST_DATA
  sfTestData = testData;
  sfReader.echoOn(); // echo back all input, by default echo is off
#endif  
  dataSerial.begin(9600);
  sfReader.connect(dataSerial); // where SafeStringReader will read from
  //sfReader.flushInput();  // use this to skip any partial data upto the first delimiter (or timeout)
}

void loop() {
  if (sfReader.read()) {
    // pick up the number
    cSF(token, 12);
    sfReader.substring(token, 1); // get the data after the P,Y,R
    float num = 0.0;
    if (!token.toFloat(num)) {
      Serial.print(F("Invalid data - ")); Serial.println(sfReader);
    } else {
      // have a valid number
      if (sfReader.startsWith('P')) {
        phonePitch = num;
        Serial.print(F("Pitch:")); Serial.println(phonePitch);
      } else if (sfReader.startsWith('Y')) {
        phoneYaw = num;
        Serial.print(F("  Yaw:")); Serial.println(phoneYaw);
      } else if (sfReader.startsWith('R')) {
        phoneRoll = num;
        Serial.print(F(" Roll:")); Serial.println(phoneRoll);
      } else {
        Serial.print(F("Unrecognized data - ")); Serial.println(sfReader);
      }
    }
  }
  // if have all three values then...
  //  EulerYPR_to_DCM(phoneYaw, phonePitch, phoneRoll, DCM);
  //  DCM_to_EulerPYR(DCM, EulerPYR);
  //  for (int i = 0; i < 3; i++) {
  //    rotPYR[i] = EulerPYR[i] - currentPYR[i];
  //    stepPYR[i] = rotPYR[i] / 1.8;
  //    currentPYR[i] = stepPYR[i] * 1.8;
  //  }
}

void EulerYPR_to_DCM(float phoneYaw, float phonePitch, float phoneRoll, float DCM [3][3]) {
  float phoneRoll_rad = phoneRoll * PI / 180;
  float phonePitch_rad = phonePitch * PI / 180;
  float phoneYaw_rad = phoneYaw * PI / 180;
  DCM[1][1] = cos(phonePitch_rad) * cos(phoneYaw_rad);
  DCM[2][1] = cos(phonePitch_rad) * sin(phoneYaw_rad);
  DCM[3][1] = -sin(phonePitch_rad);
  DCM[1][2] = -cos(phoneRoll_rad) * sin(phoneYaw_rad) + sin(phoneRoll_rad) * sin(phonePitch_rad) * cos(phoneYaw_rad);
  DCM[2][2] = cos(phoneRoll_rad) * cos(phoneYaw_rad) + sin(phoneRoll_rad) * sin(phonePitch_rad) * sin(phoneYaw_rad);
  DCM[3][2] = sin(phoneRoll_rad) * cos(phonePitch_rad);
  DCM[1][3] = sin(phoneRoll_rad) * sin(phoneYaw_rad) + cos(phoneRoll_rad) * sin(phonePitch_rad) * cos(phoneYaw_rad);
  DCM[2][3] = -sin(phoneRoll_rad) * cos(phoneYaw_rad) + cos(phoneRoll_rad) * sin(phonePitch_rad) * sin(phoneYaw_rad);
  DCM[3][3] = cos(phoneRoll_rad) * cos(phonePitch_rad);
}

void DCM_to_EulerPYR(float DCM[3][3], float EulerPYR [3]) {
  float Pitch, Yaw, Roll;
  if (DCM[2][1] < -0.99987) {
    Yaw = -90;
    Roll = 0;
    Pitch = (atan2(DCM[1][3], DCM[3][3])) * 180 / PI;
  }
  else if (DCM[2][1] > 0.99987) {
    Yaw = 90;
    Roll = 0;
    Pitch = (atan2(DCM[1][3], DCM[3][3])) * 180 / PI;
  }
  else {
    Pitch = (atan2(-DCM[3][1], DCM[1][1])) * 180 / PI;
    Yaw = (asin(DCM[2][1])) * 180 / PI;
    Roll = (atan2(-DCM[2][3], DCM[2][2])) * 180 / PI;
  }
  EulerPYR[0] = Pitch;
  EulerPYR[1] = Yaw;
  EulerPYR[2] = Roll;
}

The output from the test data is

Pitch:-101.33
  Yaw:33.55
 Roll:-34.55
Pitch:-201.33
  Yaw:133.55
 Roll:-234.55
Pitch:-1.33
  Yaw:-3.55
 Roll:-4.55
Pitch:21.33
  Yaw:53.55
 Roll:4.55
Pitch:-101.33
  Yaw:33.55
 Roll:-34.55
Pitch:-201.33
  Yaw:133.55
 Roll:-234.55
Pitch:-1.33
  Yaw:-3.55
 Roll:-4.55
Pitch:21.33
  Yaw:53.55
 Roll:4.55
\\... for ever

Some important points to note
i) SafeStringReader tokenizes on one of the specified delimiters SO.. the last entry must be followed by a delimiter. Alternatively you can specify a timeout to return the last un-delimited entry
ii) SafeStringReader has a flushInput() method to clear the input until a delimiter is read or it timesout.
Not sure is you need the triples as a group, if so then need to sync to the next 'P' entry after flushing.
Edit -- with SafeStringReader.echoOn() the data read is put back on the input to be read again so the data goes on for ever.
For real data input comment out
#define TEST_DATA
Edit -- added 64 byte buffer to sfStream and increased Serial to 115200, otherwise error msgs cause input to be skipped due to rx buffer being full.
See Arduino Serial I/O for the Real World for how to add a non-blocking serial print to avoid this problem.

#include <SoftwareSerial.h>
SoftwareSerial mySerial(2, 3);

float phonePitch = 0;
float phoneYaw = 0;
float phoneRoll = 0;

void setup()
{
  // put your setup code here, to run once:
  Serial.begin(9600);
  mySerial.begin(9600);
}

void loop()
{
  // put your main code here, to run repeatedly:
  if (mySerial.available())
  {
    char input = mySerial.read();

    switch (input)
    {
      case 'P': phonePitch = mySerial.parseFloat(); break;
      case 'Y': phoneYaw = mySerial.parseFloat(); break;
      case 'R':
        phoneRoll = mySerial.parseFloat();
        Serial.print("PYR:\t");
        Serial.print(phonePitch);
        Serial.print('\t');
        Serial.print(phoneYaw);
        Serial.print('\t');
        Serial.println(phoneRoll);
        break;

      default: break;
    }
  }
}
1 Like