Diff: main.cpp

Revision:

0:378e6e4342e9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 02 15:35:16 2017 +0000
@@ -0,0 +1,80 @@
+#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);
+}
+        
+       
+  
+