Diff: main.cpp

Revision:

0:a5b70e916def

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 07 13:33:31 2019 +0000
@@ -0,0 +1,74 @@
+#include "mbed.h"
+#include "Dynamixel.h"
+
+void getData();
+void RobotHeadControl(float x, float y, float z);
+int *SplitPos(char posString[]);
+
+Dynamixel motor(PA_15,PB_7,1000000);
+Serial device(D1, D0, 115200);
+
+char buffer[100];
+char ch;
+int index = 0;
+
+float X_axis, Y_axis, Z_axis;
+
+int main() 
+{
+    device.attach(&getData); 
+    while(1)
+    {
+        if(buffer[strlen(buffer)-1] == '/')
+        {
+            X_axis = SplitPos(buffer)[0];
+            Y_axis = SplitPos(buffer)[1];
+            Z_axis = SplitPos(buffer)[2];
+                
+            RobotHeadControl(X_axis, Y_axis, Z_axis);
+                
+            memset(buffer,0,sizeof(buffer));
+            index = 0;   
+        }            
+    }             
+}
+
+void getData()
+{
+    ch = device.getc();
+    buffer[index] = ch;
+    index++;    
+}
+
+void RobotHeadControl(float tilt, float pan, float swing)
+{
+    int speed = 400;
+    
+    //Set position of the Dynamixel Motor (Motor1(Swing):ID12, Motor2(Pan):ID3, Motor3(Tilt):ID2)
+    tilt = ((240 + tilt)*1024)/300;
+    pan = ((150 + pan)*1024)/300;
+    swing = ((150 + swing)*1024)/300;
+    
+    motor.setPosition(2, tilt, speed);
+    motor.setPosition(12, pan, speed);
+    motor.setPosition(3, swing, speed);
+}
+
+int *SplitPos(char posString[])
+{
+    int count = 0;
+    static int position[3];
+    char delim[] = "(),/";
+    char *ptr = strtok(posString, delim);
+    
+
+    while (ptr != NULL)
+    {
+        //atoi is a function to convert string to int
+        position[count] = atoi(ptr);
+        count++;
+        ptr = strtok(NULL, delim);
+    }
+    
+    return position;
+}