Pavan Kumar
/
leapbasedhapticarm
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); }