Dependencies:   Servo mbed

main.cpp

Committer:
dvpavan153
Date:
2017-05-02
Revision:
0:378e6e4342e9

File content as of revision 0:378e6e4342e9:

#include"mbed.h"
#include "Servo.h"
 
Servo myservo1(PTD0);
Servo myservo2(PTD2);
Servo myservo3(PTD3);
#include <stdio.h>
#include <string.h>
Serial pc(USBTX, USBRX); 
Serial device(PTC15, PTC14);
float map(float in, float inMin, float inMax, float outMin, float outMax) ;
int main ()
{   
    device.baud(9600);
    char buf[17];
while(1)
 {
  if (device.readable()) 
    {
   
   device.gets(buf,17);
   
   int i ;
   char *p = strtok (buf, ":");
   char *array[3];
   
        
        while (p != NULL)
            {
                array[i++] = p;
                p = strtok (NULL, ":");
            }
      
       float x,y,z;
        x = atof(array[0]);
        y = atof(array[1]);
        z = atof(array[2]);
        
        printf("x=%4f\n",x);
        printf("y=%4f\n",y);
        printf("z=%4f\n",z);
        float middle = map ( x,165.0,340.0,0.0,180.0);
        float bottom = map ( y,65.0,-85.0,0.0,180.0);
        float top=     map ( z,0.0,120.0,0.0,180.0);
        
        printf("%f\n",middle);
        myservo1 = int(middle);
        printf("%f\n",bottom);
        myservo2 = int(bottom);
         printf("%f\n",top);
        myservo3 = int(top);
        
        
        
    } 
    }
   
}
  float map(float in, float inMin, float inMax, float outMin, float outMax) {
  // check it's within the range
  if (inMin<inMax) { 
    if (in <= inMin) 
      return outMin;
    if (in >= inMax)
      return outMax;
  } else {  // cope with input range being backwards.
    if (in >= inMin) 
      return outMin;
    if (in <= inMax)
      return outMax;
  }
  // calculate how far into the range we are
  float scale = (in-inMin)/(inMax-inMin);
  // calculate the output.
  return outMin + scale*(outMax-outMin);
}