-
Notifications
You must be signed in to change notification settings - Fork 29
/
1009-cartesian_velocity_control.cc
70 lines (59 loc) · 2.08 KB
/
1009-cartesian_velocity_control.cc
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
/**
* Software License Agreement (MIT License)
*
* Copyright (c) 2022, UFACTORY, Inc.
*
* All rights reserved.
*
* @author Vinman <[email protected]> <[email protected]>
*/
#include "xarm/wrapper/xarm_api.h"
int main(int argc, char **argv) {
if (argc < 2) {
printf("Please enter IP address\n");
return 0;
}
std::string port(argv[1]);
XArmAPI *arm = new XArmAPI(port);
sleep_milliseconds(500);
if (arm->error_code != 0) arm->clean_error();
if (arm->warn_code != 0) arm->clean_warn();
arm->motion_enable(true);
arm->set_mode(0);
arm->set_state(0);
sleep_milliseconds(500);
printf("=========================================\n");
int ret;
arm->reset(true);
arm->set_mode(5);
arm->set_state(0);
sleep_milliseconds(1000);
fp32 stop_velocity[6] = {0, 0, 0, 0, 0, 0};
fp32 velocity_1[6] = {100, 0, 0, 0, 0, 0};
fp32 velocity_2[6] = {0, -100, 0, 0, 0, 0};
fp32 velocity_3[6] = {0, 0, 100, 0, 0, 0};
fp32 velocity_4[6] = {0, 100, 0, 0, 0, 0};
fp32 velocity_5[6] = {0, 0, -100, 0, 0, 0};
fp32 velocity_6[6] = {0, -100, 0, 0, 0, 0};
ret = arm->vc_set_cartesian_velocity(velocity_1);
printf("vc_set_cartesian_velocity, ret=%d\n", ret);
sleep_milliseconds(2000);
ret = arm->vc_set_cartesian_velocity(velocity_2);
printf("vc_set_cartesian_velocity, ret=%d\n", ret);
sleep_milliseconds(2000);
ret = arm->vc_set_cartesian_velocity(velocity_3);
printf("vc_set_cartesian_velocity, ret=%d\n", ret);
sleep_milliseconds(2000);
ret = arm->vc_set_cartesian_velocity(velocity_4);
printf("vc_set_cartesian_velocity, ret=%d\n", ret);
sleep_milliseconds(4000);
ret = arm->vc_set_cartesian_velocity(velocity_5);
printf("vc_set_cartesian_velocity, ret=%d\n", ret);
sleep_milliseconds(2000);
ret = arm->vc_set_cartesian_velocity(velocity_6);
printf("vc_set_cartesian_velocity, ret=%d\n", ret);
sleep_milliseconds(2000);
ret = arm->vc_set_cartesian_velocity(stop_velocity);
printf("vc_set_cartesian_velocity, ret=%d\n", ret);
return 0;
}