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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
|
#include <windows.h>
#include <stdio.h>
#include <conio.h>
#include "dynamixel.h"
#include<time.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, Hold;
int PORT_ID, GoalPos1, GoalPos2, GoalPos3, GoalPos4;
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); getch();
dxl_write_word( 4, P_GOAL_POSITION_L, 512); getch();
dxl_write_word( 1, P_GOAL_POSITION_L, 512); getch();
dxl_write_word( 244, P_GOAL_POSITION_L, 512);
Hold = 0;
printf( "My MASTER, I am ready to obey your Commands \n" );
printf("A - Pick Object A \n");
printf("B - Pick Object B \n");
printf("R - Place at Right Side \n");
printf("L - Place at Leftide \n");
printf("ESC - Exit \n");
printf("\n\n Your Choice Please, MY Master : ");
do
{ Action = getch();
switch (Action)
{ case 0x1b : break;
case 97 : if (Hold == 0)
{ Hold = 1;
dxl_write_word( 4, P_GOAL_POSITION_L, 630);
getch();
dxl_write_word( 5, P_GOAL_POSITION_L, 176);
getch();
dxl_write_word( 1, P_GOAL_POSITION_L, 625);
getch();
dxl_write_word( 4, P_GOAL_POSITION_L, 512);
printf("Yes, My Master, Where should I Place it?"); }
else
{ printf("Sorry My Master! I Couldn't - I am Holding an Object! \n\n");
} break;
case 98 : if (Hold == 0)
{ Hold = 1;
dxl_write_word( 4, P_GOAL_POSITION_L, 630);
getch();
dxl_write_word( 5, P_GOAL_POSITION_L, 176);
getch();
dxl_write_word( 1, P_GOAL_POSITION_L, 625);
getch();
dxl_write_word( 4, P_GOAL_POSITION_L, 512);
printf("Yes, My Master, Where should I Place it?");}
else
{ printf("Sorry My Master! I Couldn't - I am Holding an Object! \n\n");
} break;
case 114 : if (Hold == 1)
{ Hold = 0;
dxl_write_word( 244, P_GOAL_POSITION_L, 218);
getch();
dxl_write_word( 4, P_GOAL_POSITION_L, 635);
getch();
dxl_write_word( 1, P_GOAL_POSITION_L, 436);
getch();
dxl_write_word( 4, P_GOAL_POSITION_L, 512);
getch();
dxl_write_word(244, P_GOAL_POSITION_L, 512);
printf("I think that I have done the JOB correctly!"); }
else
{ printf("Sorry My Master! I Couldn't - I don't have anything to place! \n\n");
} break;
case 108 : if (Hold == 1)
{ Hold = 0;
dxl_write_word(244, P_GOAL_POSITION_L, 839);
getch();
dxl_write_word( 4, P_GOAL_POSITION_L, 635);
getch();
dxl_write_word( 1, P_GOAL_POSITION_L, 436);
getch();
dxl_write_word( 4, P_GOAL_POSITION_L, 512);
getch();
dxl_write_word( 244, P_GOAL_POSITION_L, 512);
printf("I think that I have done the JOB correctly!");}
else
{ printf("Sorry My Master! I Couldn't - I don't have anything to place! \n\n");
} break;
default: printf(" Sorry, I don't have this Knowledge! - Teach me My Master! \n\n"); break;
}
}while(Action != 0x1b);
// Close device
dxl_terminate();
printf( "Press any key to terminate...\n" );
getch();
return 0;
}
|
