First program commit
Dependencies: mbed
Revision 0:a5b70e916def, committed 2019-02-07
- Comitter:
- tengtingtong
- Date:
- Thu Feb 07 13:33:31 2019 +0000
- Commit message:
- First commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libz_dynamixel.lib Thu Feb 07 13:33:31 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Surrogate-Robot/code/libz_dynamixel/#c93069e41792
--- /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; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Feb 07 13:33:31 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file