File content as of revision 2:0daf6588a802:

#include "mbed.h"
#include "sMotor.h"
#include <string>
 
sMotor motor(dp9, dp10, dp11, dp13); // creates new stepper motor: IN1, IN2, IN3, IN4
Serial pc(USBTX, USBRX);

int step_speed = 1200 ; // set default motor speed
int numstep = 512 ; // defines full turn of 360 degree
int direction = 0; //0 for right, 1 for left

AnalogIn sensor(dp23);

enum State {Idle, Working};

State state = Idle;
int index = 0;
int buffer_size = 300;
int samples_num = 13;
float distances[13] = {15.0, 20.0, 25.0, 30.0, 35.0, 40.0, 45.0, 50.0, 55.0, 60.0, 65.0, 70.0, 75.0};
float voltages[13] = {0.78, 0.72, 0.63, 0.55, 0.47, 0.43, 0.39, 0.36, 0.34, 0.33, 0.32, 0.311, 0.31};
float value;
int angle(0);
float scale (1 / (numstep/360.0));

string ToString(float num)
{
    if (num < 0.0)
    return "-1.0";
    char number [4];
    number[0] = num/10 + '0';
    number[1] = int(num) % 10 + '0';
    number[2] = '.';
    number[3] = int((num - int(num)) * 10) + '0';
    string Result = number;
    return Result;
}

void MoveMotor()
{
   motor.step(int(numstep / 360.0 * scale * (angle  % 360) + 0.5), direction, step_speed); // number of steps, direction, speed
   angle += 2;
   index++;
   index = index % buffer_size;
}

void CheckState()
{
     if (pc.readable())
    {
       string command;
       pc.scanf("%s", &command);
       switch (state)
     {
        case Idle:
         if (command == "Test")
          pc.printf("Okay\n");
         else if (command == "Start")
            {
                pc.printf("Started\n");
                state = Working;
            }

       break;
       case Working:
        if (command == "Stop")
        {
          pc.printf("Stopped\n");
          state = Idle;
        }
        break;
    }
    }
}

float InterpolatedValue( int i )
{
    float alpha = (value - voltages[i]) / (voltages[i + 1] - voltages[i]);
    return (1 - alpha)* voltages[i] + alpha*voltages[i + 1];
}

void GenerateValue()
{
      string distance;

      value = sensor;
      if( value < 0.0001)
      value = -1.0;
      else
      for( int i = 0; i < samples_num - 1; i++)
      {
          if( value > voltages[0])
      {
          value = 10;
          break;
      }
      else if( value >= voltages[i] && value <= voltages[i + 1])
      {
          value = InterpolatedValue(i);
          break;
      }
      }
     
     distance = ToString(value);
      
      pc.printf("%s\n",distance);    
}
int main() {
    
    while (1) {   
                CheckState();
                if (state == Working)
                {
                    GenerateValue();
                    MoveMotor();
                    wait_ms(40);             
                }
            }
        }