Skip to content

Commit 861d8e4

Browse files
KyoungHwan2046KyoungHwan2046
authored
Merge pull request #231 from ROBOTIS-GIT/develop
Develop
2 parents 8e52694 + 73fc871 commit 861d8e4

File tree

4 files changed

+409
-13
lines changed

4 files changed

+409
-13
lines changed

‎arduino/opencr_arduino/opencr/libraries/OpenManipulator/example/Chain/open_manipulator_chain_teaching/open_manipulator_chain_teaching.ino

+346
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,346 @@
1+
/*******************************************************************************
2+
* Copyright 2018 ROBOTIS CO., LTD.
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*******************************************************************************/
16+
17+
/* Authors: Will Son */
18+
19+
#include <open_manipulator_libs.h>
20+
#include "Arduino.h"
21+
22+
OpenManipulator open_manipulator;
23+
double control_time = 0.010; //default control frequency (100Hz)
24+
double present_time = 0.0;
25+
double previous_time = 0.0;
26+
bool platform_state = true;
27+
bool start_motion_flag = false;
28+
bool stop_motion_flag = false;
29+
bool teaching_mode_flag = false;
30+
uint8_t motion_cnt[] = {0};
31+
uint8_t motion_index = 0;
32+
uint8_t motion_number = 0;
33+
34+
std::vector<JointValue> present_position;
35+
std::vector<JointValue> gripper_position;
36+
std::vector<JointValue> saved_teaching_pose;
37+
38+
void setup()
39+
{
40+
Serial.begin(57600);
41+
while (!Serial); // Wait until Serial is Opened
42+
43+
open_manipulator.setOpenManipulatorCustomJointId(11, 12, 13, 14, 15); // ID 11, 12, 13, 14, 15 is default
44+
open_manipulator.initOpenManipulator(platform_state);
45+
Serial.println("OpenManipulator Init Begin");
46+
47+
initDemo();
48+
49+
Serial.println("Press [SW1] to start programmed motion. Press [SW2] to start teaching mode.");
50+
while (true)
51+
{
52+
digitalWrite(BDPIN_LED_USER_1, LOW);
53+
if(digitalRead(BDPIN_PUSH_SW_1))
54+
{
55+
Serial.println("Programmed motion mode begin.");
56+
teaching_mode_flag = false;
57+
warning();
58+
break;
59+
}
60+
else if(digitalRead(BDPIN_PUSH_SW_2))
61+
{
62+
Serial.println("Teaching mode begin.");
63+
teaching_mode_flag = true;
64+
break;
65+
}
66+
}
67+
digitalWrite(BDPIN_LED_USER_1, HIGH);
68+
}
69+
70+
void loop()
71+
{
72+
if (teaching_mode_flag)
73+
{
74+
open_manipulator.disableAllActuator();
75+
delay_ms(1000);
76+
Serial.println("Press [SW2] to append Current Pose. Press [SW1] to finish appending pose and play taught pose");
77+
78+
while (true)
79+
{
80+
digitalWrite(BDPIN_LED_USER_2, LOW);
81+
if (digitalRead(BDPIN_PUSH_SW_2))
82+
{
83+
// Append(Teach) the current Pose to the present_position data
84+
uint8_t pos_vector_index = motion_index;
85+
present_position = open_manipulator.receiveAllJointActuatorValue();
86+
gripper_position = open_manipulator.receiveAllToolActuatorValue();
87+
for(uint8_t joint_index = 0; joint_index < 4; joint_index++ )
88+
{
89+
saved_teaching_pose.push_back(present_position.at(joint_index));
90+
}
91+
saved_teaching_pose.push_back(gripper_position.at(0));
92+
93+
// Display saved Pose Joint angles in Radian
94+
Serial.print("[Pose ");
95+
Serial.print(motion_index/5 + 1);
96+
Serial.print("] : ");
97+
Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
98+
Serial.print(", ");
99+
Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
100+
Serial.print(", ");
101+
Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
102+
Serial.print(", ");
103+
Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
104+
Serial.print(", ");
105+
Serial.println(saved_teaching_pose[pos_vector_index++].position, 3);
106+
motion_index = motion_index + 5;
107+
break;
108+
}
109+
if (digitalRead(BDPIN_PUSH_SW_1))
110+
{
111+
digitalWrite(BDPIN_LED_USER_3, LOW);
112+
teaching_mode_flag = false;
113+
114+
// Display the list of saved(taught) Pose
115+
for(uint8_t index = 0; index < motion_index ; index++)
116+
{
117+
if(index % 5 == 0)
118+
{
119+
Serial.println();
120+
}
121+
else
122+
{
123+
Serial.print(", ");
124+
}
125+
Serial.print(saved_teaching_pose[index].position, 3);
126+
}
127+
128+
warning();
129+
break;
130+
}
131+
}
132+
digitalWrite(BDPIN_LED_USER_2, HIGH);
133+
}
134+
else
135+
{
136+
present_time = millis()/1000.0;
137+
138+
// Trajectory following movement occurs here
139+
if(present_time-previous_time >= control_time)
140+
{
141+
open_manipulator.processOpenManipulator(millis()/1000.0);
142+
previous_time = millis()/1000.0;
143+
}
144+
145+
// Read the next Pose to move
146+
if(!saved_teaching_pose.empty())
147+
{
148+
runTeachingMotion(&open_manipulator);
149+
}
150+
else
151+
{
152+
runDemo(&open_manipulator);
153+
}
154+
}
155+
}
156+
157+
// Output warning before starting motion
158+
void warning()
159+
{
160+
Serial.println();
161+
Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 5 seconds.");
162+
delay_ms(1000);
163+
Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 4 seconds.");
164+
delay_ms(1000);
165+
Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 3 seconds.");
166+
delay_ms(1000);
167+
Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 2 seconds.");
168+
delay_ms(1000);
169+
Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 1 seconds.");
170+
open_manipulator.receiveAllJointActuatorValue();
171+
open_manipulator.receiveAllToolActuatorValue();
172+
open_manipulator.enableAllActuator();
173+
delay_ms(1000);
174+
}
175+
176+
// Move in Joint Space
177+
void moveJS(OpenManipulator *open_manipulator, double j1, double j2, double j3, double j4, double gripper_pos, double time)
178+
{
179+
static std::vector <double> target_angle;
180+
target_angle.clear();
181+
target_angle.push_back(j1);
182+
target_angle.push_back(j2);
183+
target_angle.push_back(j3);
184+
target_angle.push_back(j4);
185+
open_manipulator->makeJointTrajectory(target_angle, time);
186+
open_manipulator->makeToolTrajectory("gripper", gripper_pos);
187+
}
188+
189+
/*****************************************************************************
190+
** Initialize Demo
191+
*****************************************************************************/
192+
void initDemo()
193+
{
194+
start_motion_flag = false;
195+
motion_cnt[0] = 0;
196+
197+
pinMode(BDPIN_PUSH_SW_1, INPUT);
198+
pinMode(BDPIN_PUSH_SW_2, INPUT);
199+
pinMode(BDPIN_GPIO_1, OUTPUT);
200+
digitalWrite(BDPIN_GPIO_1, LOW);
201+
Serial.println("OpenManipulator Init Completed");
202+
}
203+
204+
/*****************************************************************************
205+
** Play Demo Motion
206+
*****************************************************************************/
207+
void runDemo(OpenManipulator *open_manipulator)
208+
{
209+
if(!start_motion_flag && !stop_motion_flag)
210+
{
211+
startMotion();
212+
}
213+
if(!start_motion_flag && stop_motion_flag) // Restart DEMO
214+
{
215+
if(digitalRead(BDPIN_PUSH_SW_1))
216+
{
217+
startMotion();
218+
}
219+
}
220+
if(digitalRead(BDPIN_PUSH_SW_2) && !stop_motion_flag) // Stop DEMO
221+
{
222+
stopMotion(open_manipulator);
223+
}
224+
225+
if (open_manipulator->getMovingState())
226+
{
227+
return;
228+
}
229+
else
230+
{
231+
if (start_motion_flag)
232+
{
233+
switch(motion_cnt[0])
234+
{
235+
case 0:
236+
moveJS(open_manipulator, 0.0, -1.0, 0.2, 0.8, 0.0, 2.0);
237+
motion_cnt[0] ++;
238+
break;
239+
case 1:
240+
moveJS(open_manipulator, -0.5, 0.0, 1.0, -1.0, 0.01, 2.0);
241+
motion_cnt[0] ++;
242+
break;
243+
case 2:
244+
moveJS(open_manipulator, -0.5, 0.1, 0.75, -0.85, 0.01, 2.0);
245+
motion_cnt[0] ++;
246+
break;
247+
case 3:
248+
open_manipulator->sleepTrajectory(2.0);
249+
motion_cnt[0] ++;
250+
break;
251+
case 4:
252+
moveJS(open_manipulator, -0.5, -0.05, 1.05, -1.0, 0.0, 2.0);
253+
motion_cnt[0] = 0;
254+
break;
255+
default:
256+
motion_cnt[0] = 0;
257+
break;
258+
}
259+
}
260+
}
261+
}
262+
263+
/*****************************************************************************
264+
** Play Teaching Motion
265+
*****************************************************************************/
266+
void runTeachingMotion(OpenManipulator *open_manipulator)
267+
{
268+
if(!start_motion_flag && !stop_motion_flag) // Start Teach Motion
269+
{
270+
startMotion();
271+
}
272+
if(!start_motion_flag && stop_motion_flag) // Restart Teach Motion
273+
{
274+
if(digitalRead(BDPIN_PUSH_SW_1))
275+
{
276+
startMotion();
277+
}
278+
}
279+
if(digitalRead(BDPIN_PUSH_SW_2) && !stop_motion_flag) // Stop Teach Motion
280+
{
281+
stopMotion(open_manipulator);
282+
digitalWrite(BDPIN_LED_USER_1, HIGH); // Turn off USER1 LED
283+
digitalWrite(BDPIN_LED_USER_2, HIGH); // Turn off USER2 LED
284+
digitalWrite(BDPIN_LED_USER_3, HIGH); // Turn off USER3 LED
285+
digitalWrite(BDPIN_LED_USER_4, HIGH); // Turn off USER4 LED
286+
}
287+
288+
if (open_manipulator->getMovingState()) // Check if OpenMANIPULATOR is moving along the trajectory
289+
{
290+
// If OpenMANIPULATOR is still moving along the trajectory, then do not get the next Pose and return to main loop
291+
// Digital Output HIGH(3.3V) on OpenCR GPIO pin #3 (http://emanual.robotis.com/docs/en/parts/controller/opencr10/#gpio)
292+
digitalWrite(BDPIN_GPIO_1, HIGH);
293+
return;
294+
}
295+
else
296+
{
297+
// Digital Output LOW(0V) on OpenCR GPIO pin #3 (http://emanual.robotis.com/docs/en/parts/controller/opencr10/#gpio)
298+
digitalWrite(BDPIN_GPIO_1, LOW);
299+
if (start_motion_flag)
300+
{
301+
if (motion_cnt[0] < motion_index)
302+
{
303+
uint8_t joint1_index = motion_cnt[0]++;
304+
uint8_t joint2_index = motion_cnt[0]++;
305+
uint8_t joint3_index = motion_cnt[0]++;
306+
uint8_t joint4_index = motion_cnt[0]++;
307+
uint8_t gripper_index = motion_cnt[0]++;
308+
309+
// Play each motion during 3.0 seconds
310+
moveJS(open_manipulator, saved_teaching_pose[joint1_index].position, saved_teaching_pose[joint2_index].position, saved_teaching_pose[joint3_index].position, saved_teaching_pose[joint4_index].position, saved_teaching_pose[gripper_index].position, 3.0);
311+
}
312+
else
313+
{
314+
motion_cnt[0] = 0;
315+
}
316+
}
317+
}
318+
}
319+
320+
/*****************************************************************************
321+
** Start Motion
322+
*****************************************************************************/
323+
void startMotion()
324+
{
325+
Serial.println("Press [SW2] to Stop Motion");
326+
// Start the motion
327+
start_motion_flag = true;
328+
stop_motion_flag = false;
329+
330+
motion_cnt[0] = 0;
331+
}
332+
333+
/*****************************************************************************
334+
** Stop Motion
335+
*****************************************************************************/
336+
void stopMotion(OpenManipulator *open_manipulator)
337+
{
338+
Serial.println("Press [SW1] to Start Motion");
339+
// Stop the motion
340+
start_motion_flag = false;
341+
stop_motion_flag = true;
342+
343+
// Go to the Init Pose(Right angle pose).
344+
moveJS(open_manipulator, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0);
345+
motion_cnt[0] = 0;
346+
}

‎arduino/opencr_arduino/opencr/libraries/OpenManipulator/src/open_manipulator_libs/include/open_manipulator_libs/open_manipulator.h

+4
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ class OpenManipulator : public robotis_manipulator::RobotisManipulator
4444
robotis_manipulator::JointActuator *joint_;
4545
robotis_manipulator::ToolActuator *tool_;
4646
robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE];
47+
uint8_t joint1_id, joint2_id, joint3_id, joint4_id, gripper_id;
4748

4849
public:
4950
OpenManipulator();
@@ -54,6 +55,9 @@ class OpenManipulator : public robotis_manipulator::RobotisManipulator
5455
STRING baud_rate = "1000000",
5556
float control_loop_time = 0.010);
5657
void processOpenManipulator(double present_time);
58+
59+
// Allow to use a custom joint ID for OpenMANIPULATOR-X
60+
void setOpenManipulatorCustomJointId(uint8_t joint1, uint8_t joint2, uint8_t joint3, uint8_t joint4, uint8_t gripper);
5761
};
5862

5963
#endif // OPEN_MANIPULTOR_H_

0 commit comments

Comments
 (0)