Surrogate Robot / Mbed 2 deprecated DynamixelAX-12a

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:a5b70e916def, 2019-02-07 (annotated)

Committer:
tengtingtong
Date:
Thu Feb 07 13:33:31 2019 +0000
Revision:
0:a5b70e916def
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tengtingtong 0:a5b70e916def 1 #include "mbed.h"
tengtingtong 0:a5b70e916def 2 #include "Dynamixel.h"
tengtingtong 0:a5b70e916def 3
tengtingtong 0:a5b70e916def 4 void getData();
tengtingtong 0:a5b70e916def 5 void RobotHeadControl(float x, float y, float z);
tengtingtong 0:a5b70e916def 6 int *SplitPos(char posString[]);
tengtingtong 0:a5b70e916def 7
tengtingtong 0:a5b70e916def 8 Dynamixel motor(PA_15,PB_7,1000000);
tengtingtong 0:a5b70e916def 9 Serial device(D1, D0, 115200);
tengtingtong 0:a5b70e916def 10
tengtingtong 0:a5b70e916def 11 char buffer[100];
tengtingtong 0:a5b70e916def 12 char ch;
tengtingtong 0:a5b70e916def 13 int index = 0;
tengtingtong 0:a5b70e916def 14
tengtingtong 0:a5b70e916def 15 float X_axis, Y_axis, Z_axis;
tengtingtong 0:a5b70e916def 16
tengtingtong 0:a5b70e916def 17 int main()
tengtingtong 0:a5b70e916def 18 {
tengtingtong 0:a5b70e916def 19 device.attach(&getData);
tengtingtong 0:a5b70e916def 20 while(1)
tengtingtong 0:a5b70e916def 21 {
tengtingtong 0:a5b70e916def 22 if(buffer[strlen(buffer)-1] == '/')
tengtingtong 0:a5b70e916def 23 {
tengtingtong 0:a5b70e916def 24 X_axis = SplitPos(buffer)[0];
tengtingtong 0:a5b70e916def 25 Y_axis = SplitPos(buffer)[1];
tengtingtong 0:a5b70e916def 26 Z_axis = SplitPos(buffer)[2];
tengtingtong 0:a5b70e916def 27
tengtingtong 0:a5b70e916def 28 RobotHeadControl(X_axis, Y_axis, Z_axis);
tengtingtong 0:a5b70e916def 29
tengtingtong 0:a5b70e916def 30 memset(buffer,0,sizeof(buffer));
tengtingtong 0:a5b70e916def 31 index = 0;
tengtingtong 0:a5b70e916def 32 }
tengtingtong 0:a5b70e916def 33 }
tengtingtong 0:a5b70e916def 34 }
tengtingtong 0:a5b70e916def 35
tengtingtong 0:a5b70e916def 36 void getData()
tengtingtong 0:a5b70e916def 37 {
tengtingtong 0:a5b70e916def 38 ch = device.getc();
tengtingtong 0:a5b70e916def 39 buffer[index] = ch;
tengtingtong 0:a5b70e916def 40 index++;
tengtingtong 0:a5b70e916def 41 }
tengtingtong 0:a5b70e916def 42
tengtingtong 0:a5b70e916def 43 void RobotHeadControl(float tilt, float pan, float swing)
tengtingtong 0:a5b70e916def 44 {
tengtingtong 0:a5b70e916def 45 int speed = 400;
tengtingtong 0:a5b70e916def 46
tengtingtong 0:a5b70e916def 47 //Set position of the Dynamixel Motor (Motor1(Swing):ID12, Motor2(Pan):ID3, Motor3(Tilt):ID2)
tengtingtong 0:a5b70e916def 48 tilt = ((240 + tilt)*1024)/300;
tengtingtong 0:a5b70e916def 49 pan = ((150 + pan)*1024)/300;
tengtingtong 0:a5b70e916def 50 swing = ((150 + swing)*1024)/300;
tengtingtong 0:a5b70e916def 51
tengtingtong 0:a5b70e916def 52 motor.setPosition(2, tilt, speed);
tengtingtong 0:a5b70e916def 53 motor.setPosition(12, pan, speed);
tengtingtong 0:a5b70e916def 54 motor.setPosition(3, swing, speed);
tengtingtong 0:a5b70e916def 55 }
tengtingtong 0:a5b70e916def 56
tengtingtong 0:a5b70e916def 57 int *SplitPos(char posString[])
tengtingtong 0:a5b70e916def 58 {
tengtingtong 0:a5b70e916def 59 int count = 0;
tengtingtong 0:a5b70e916def 60 static int position[3];
tengtingtong 0:a5b70e916def 61 char delim[] = "(),/";
tengtingtong 0:a5b70e916def 62 char *ptr = strtok(posString, delim);
tengtingtong 0:a5b70e916def 63
tengtingtong 0:a5b70e916def 64
tengtingtong 0:a5b70e916def 65 while (ptr != NULL)
tengtingtong 0:a5b70e916def 66 {
tengtingtong 0:a5b70e916def 67 //atoi is a function to convert string to int
tengtingtong 0:a5b70e916def 68 position[count] = atoi(ptr);
tengtingtong 0:a5b70e916def 69 count++;
tengtingtong 0:a5b70e916def 70 ptr = strtok(NULL, delim);
tengtingtong 0:a5b70e916def 71 }
tengtingtong 0:a5b70e916def 72
tengtingtong 0:a5b70e916def 73 return position;
tengtingtong 0:a5b70e916def 74 }