1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
#include <windows.h>
#include <stdio.h>
#include <conio.h>
#include "dynamixel.h"
#pragma comment(lib, "dynamixel.lib")
// Control table address
#define P_GOAL_POSITION_L 30
#define P_GOAL_POSITION_H 31
#define P_PRESENT_POSITION_L 36
#define P_PRESENT_POSITION_H 37
#define P_MOVING 46
// Defulat setting
#define DEFAULT_PORTNUM 25 // COM3
#define DEFAULT_BAUDNUM 1 // 1Mbps
int main()
{
int Moving, PresentPos;
int CommStatus;
int PORT_ID, GoalPos1=512, GoalPos2=512, GoalPos3=512, GoalPos4=512;
char Action;
// Open device
if( dxl_initialize(DEFAULT_PORTNUM, DEFAULT_BAUDNUM) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press any key to terminate...\n" );
getch();
return 0;
}
else
printf( "System Ready to Control ROBOT!\n" );
//initial position
dxl_write_word( 5, P_GOAL_POSITION_L, 512);
dxl_write_word( 4, P_GOAL_POSITION_L, 512);
dxl_write_word( 1, P_GOAL_POSITION_L, 512);
dxl_write_word( 244, P_GOAL_POSITION_L, 512);
printf( "Control me MY MASTER using control Keys\n" );
do
{
Action = getch();
switch (Action)
{ case 0x1b : break;
case 13 : PORT_ID = 5;
GoalPos1 = GoalPos1 + 10;
// Write goal position
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos1); break;
case 48 : PORT_ID = 5;
GoalPos1 = GoalPos1 - 10;
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos1); break;
case 56 : PORT_ID = 4;
GoalPos2 = GoalPos2 + 10;
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos2); break;
case 50 : PORT_ID = 4;
GoalPos2 = GoalPos2 - 10;
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos2); break;
case 43 : PORT_ID = 1;
GoalPos3 = GoalPos3 + 10;
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos3); break;
case 45 : PORT_ID = 1;
GoalPos3 = GoalPos3 - 10;
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos3); break;
case 52 : PORT_ID = 244;
GoalPos4 = GoalPos4 + 10;
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos4); break;
case 54 : PORT_ID = 244;
GoalPos4 = GoalPos4 - 10;
dxl_write_word( PORT_ID, P_GOAL_POSITION_L, GoalPos4);
break;
default: printf(" Sorry not a Control Key - Try again!"); break;
}
}while(Action != 0x1b);
// Close device
dxl_terminate();
printf( "Press any key to terminate...\n" );
getch();
return 0;
}
|
