DIY - tensionneur de harnais 2 DOF avec PTMover/simHub

/*
  Original code written by sirnoname http://www.x-sim.de/
  Thanks to vthinsel for help and some functions
  4 axis version by Wanegain Racingfr.net
  Edited for servomotor ASME-04 command by MikeTheBike71

  Written for Simtools (xsimulator.net) using Arduino UNO/MEGA...

  10 bits (16 bits value / 64)

  57600 bauds

  Interface Output :
  X1<Axis1a>CX2<Axis2a>CX3<Axis3a>CX4<Axis4a>C

  Start :
  No need

  Stop :
  No need
*/

#include <Servo.h>

int servo1 = 47; // Pins to which the servos are attached
int servo2 = 49; // Digital pins 47, 49, 51 and 53 in my case (Arduino MEGA)
int servo3 = 51;
int servo4 = 53;

Servo myServo1; // create a servo object
Servo myServo2;
Servo myServo3;
Servo myServo4;

int buffer = 0;
int buffercount = -1;
int commandbuffer[5] = {0};

float target1 = 512; // target send by Simhub
float target2 = 512;
float target3 = 512;
float target4 = 512;

float targetAcos1 = 0;
float targetAcos2 = 0;
float targetAcos3 = 0;
float targetAcos4 = 0;

float angleTargetRad1 = 0; // Compute linear setpoint out of circular setpoint (Servomotor)
float angleTargetRad2 = 0;
float angleTargetRad3 = 0;
float angleTargetRad4 = 0;

int microsec1 = 0; // Target (0 to 1023) converted into microSeconds to drive the servomotors precisely (better than angle)
int microsec2 = 0;
int microsec3 = 0;
int microsec4 = 0;

int offset1 = 720; // Offset of servomotors ASME-04 for having the good low and high point.
int offset2 = 700; // 500 to 2500µsec for 300deg but only 180deg (1200µsec) used in our case
int offset3 = 740; // So 500 to 1300 used to set offset 
int offset4 = 740; // Usefull to have the start position right where you want it

//+-----------------------+
//|         SETUP         |
//+-----------------------+
void setup()
{
  Serial.begin(57600, SERIAL_8N1); //Simtools --> Arduino //can be set faster but not needed

  myServo1.attach(servo1); // attaches the servoX on pin servoX to the servo objectX
  myServo2.attach(servo2);
  myServo3.attach(servo3);
  myServo4.attach(servo4);

} //----- SETUP -----

//+-------------------------------+
//|         SerialWorker          |
//+-------------------------------+
void SerialWorker()
{
  while (Serial.available())
  {
    if (buffercount == -1)
    {
      buffer = Serial.read();
      if (buffer != 'X') {
        buffercount = -1;
      } else {
        buffercount = 0;
      }
    }
    else
    {
      buffer = Serial.read();
      commandbuffer[buffercount] = buffer;
      buffercount++;
      if (buffercount > 3)
      {
        if (commandbuffer[3] == 'C')
        {
          ParseCommand();
        }
        buffercount = -1;
      }
    }
  }
}//----- SERIALWORKER -----

//+-------------------------------+
//|         ParseCommand          |
//+-------------------------------+
void ParseCommand()
{

  if (commandbuffer[0] == '1')			//Set motor 1 position to High and Low value 0 to 1023
  {
    target1 = (commandbuffer[1] * 256) + commandbuffer[2]; //Raw data from software
    return;
  }
  if (commandbuffer[0] == '2')			//Set motor 2 position to High and Low value 0 to 1023
  {
    target2 = (commandbuffer[1] * 256) + commandbuffer[2];
    return;
  }
  if (commandbuffer[0] == '3')			//Set motor 3 position to High and Low value 0 to 1023
  {
    target3 = (commandbuffer[1] * 256) + commandbuffer[2];
    return;
  }
  if (commandbuffer[0] == '4')			//Set motor 4 position to High and Low value 0 to 1023
  {
    target4 = (commandbuffer[1] * 256) + commandbuffer[2];
    return;
  }
}

//+-------------------------------+
//|   Circular/Linear_Conversion  |
//+-------------------------------+
void Conversion() // Convert circular setpoint into linear setpoint (For having linear response on servomotor)
{
    targetAcos1 = -(target1 - 511)/512;
    targetAcos2 = -(target2 - 511)/512;
    targetAcos3 = -(target3 - 511)/512;
    targetAcos4 = -(target4 - 511)/512;

    angleTargetRad1 = acos(targetAcos1); // Compute adjacent side length of right triangle
    angleTargetRad2 = acos(targetAcos2); // Compute adjacent side length of right triangle
    angleTargetRad3 = acos(targetAcos3); // Compute adjacent side length of right triangle
    angleTargetRad4 = acos(targetAcos4); // Compute adjacent side length of right triangle

    microsec1 = angleTargetRad1 * 1200 / PI;
    microsec2 = angleTargetRad2 * 1200 / PI;
    microsec3 = angleTargetRad3 * 1200 / PI;
    microsec4 = angleTargetRad4 * 1200 / PI;

}

//+-------------------------------+
//|         Servos_Command        |
//+-------------------------------+
void servos_Command() // Servos command
{
    myServo1.writeMicroseconds(microsec1 + offset1); // set the servo position
    myServo2.writeMicroseconds(microsec2 + offset2);
    myServo3.writeMicroseconds(microsec3 + offset3);
    myServo4.writeMicroseconds(microsec4 + offset4);
}

//#########################
//#         loop          #
//#########################
void loop()
{
  //Program loop
  while (1 == 1) //Important hack: Use this own real time loop code without arduino framework delays
  {
    SerialWorker(); // Serial data reading

    Conversion(); // Convert linear setpoint into circular setpoint because of servomotor use
 
    servos_Command(); // Servos command

  }//while (1 == 1)
}//----- LOOP -----