Merge pull request #231 from ROBOTIS-GIT/develop

Develop

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

@@ -0,0 +1,346 @@
+/*******************************************************************************
+* Copyright 2018 ROBOTIS CO., LTD.
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http:https://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*******************************************************************************/
+
+/* Authors: Will Son */
+
+#include <open_manipulator_libs.h>
+#include "Arduino.h"
+
+OpenManipulator open_manipulator;
+double control_time = 0.010; //default control frequency (100Hz)
+double present_time = 0.0;
+double previous_time = 0.0;
+bool platform_state = true;
+bool start_motion_flag = false;
+bool stop_motion_flag = false;
+bool teaching_mode_flag = false;
+uint8_t motion_cnt[] = {0};
+uint8_t motion_index = 0;
+uint8_t motion_number = 0;
+
+std::vector<JointValue> present_position;
+std::vector<JointValue> gripper_position;
+std::vector<JointValue> saved_teaching_pose;
+
+void setup()
+{
+ Serial.begin(57600);
+ while (!Serial); // Wait until Serial is Opened
+
+ open_manipulator.setOpenManipulatorCustomJointId(11, 12, 13, 14, 15); // ID 11, 12, 13, 14, 15 is default
+ open_manipulator.initOpenManipulator(platform_state);
+ Serial.println("OpenManipulator Init Begin");
+
+ initDemo();
+
+ Serial.println("Press [SW1] to start programmed motion. Press [SW2] to start teaching mode.");
+ while (true)
+ {
+ digitalWrite(BDPIN_LED_USER_1, LOW);
+ if(digitalRead(BDPIN_PUSH_SW_1))
+ {
+ Serial.println("Programmed motion mode begin.");
+ teaching_mode_flag = false;
+ warning();
+ break;
+ }
+ else if(digitalRead(BDPIN_PUSH_SW_2))
+ {
+ Serial.println("Teaching mode begin.");
+ teaching_mode_flag = true;
+ break;
+ }
+ }
+ digitalWrite(BDPIN_LED_USER_1, HIGH);
+}
+
+void loop()
+{
+ if (teaching_mode_flag)
+ {
+ open_manipulator.disableAllActuator();
+ delay_ms(1000);
+ Serial.println("Press [SW2] to append Current Pose. Press [SW1] to finish appending pose and play taught pose");
+
+ while (true)
+ {
+ digitalWrite(BDPIN_LED_USER_2, LOW);
+ if (digitalRead(BDPIN_PUSH_SW_2))
+ {
+ // Append(Teach) the current Pose to the present_position data
+ uint8_t pos_vector_index = motion_index;
+ present_position = open_manipulator.receiveAllJointActuatorValue();
+ gripper_position = open_manipulator.receiveAllToolActuatorValue();
+ for(uint8_t joint_index = 0; joint_index < 4; joint_index++ )
+ {
+ saved_teaching_pose.push_back(present_position.at(joint_index));
+ }
+ saved_teaching_pose.push_back(gripper_position.at(0));
+
+ // Display saved Pose Joint angles in Radian
+ Serial.print("[Pose ");
+ Serial.print(motion_index/5 + 1);
+ Serial.print("] : ");
+ Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
+ Serial.print(", ");
+ Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
+ Serial.print(", ");
+ Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
+ Serial.print(", ");
+ Serial.print(saved_teaching_pose[pos_vector_index++].position, 3);
+ Serial.print(", ");
+ Serial.println(saved_teaching_pose[pos_vector_index++].position, 3);
+ motion_index = motion_index + 5;
+ break;
+ }
+ if (digitalRead(BDPIN_PUSH_SW_1))
+ {
+ digitalWrite(BDPIN_LED_USER_3, LOW);
+ teaching_mode_flag = false;
+
+ // Display the list of saved(taught) Pose
+ for(uint8_t index = 0; index < motion_index ; index++)
+ {
+ if(index % 5 == 0)
+ {
+ Serial.println();
+ }
+ else
+ {
+ Serial.print(", ");
+ }
+ Serial.print(saved_teaching_pose[index].position, 3);
+ }
+
+ warning();
+ break;
+ }
+ }
+ digitalWrite(BDPIN_LED_USER_2, HIGH);
+ }
+ else
+ {
+ present_time = millis()/1000.0;
+
+ // Trajectory following movement occurs here
+ if(present_time-previous_time >= control_time)
+ {
+ open_manipulator.processOpenManipulator(millis()/1000.0);
+ previous_time = millis()/1000.0;
+ }
+
+ // Read the next Pose to move
+ if(!saved_teaching_pose.empty())
+ {
+ runTeachingMotion(&open_manipulator);
+ }
+ else
+ {
+ runDemo(&open_manipulator);
+ }
+ }
+}
+
+// Output warning before starting motion
+void warning()
+{
+ Serial.println();
+ Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 5 seconds.");
+ delay_ms(1000);
+ Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 4 seconds.");
+ delay_ms(1000);
+ Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 3 seconds.");
+ delay_ms(1000);
+ Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 2 seconds.");
+ delay_ms(1000);
+ Serial.println("WARNING!!! OpenMANIPULATOR-X operates in 1 seconds.");
+ open_manipulator.receiveAllJointActuatorValue();
+ open_manipulator.receiveAllToolActuatorValue();
+ open_manipulator.enableAllActuator();
+ delay_ms(1000);
+}
+
+// Move in Joint Space 
+void moveJS(OpenManipulator *open_manipulator, double j1, double j2, double j3, double j4, double gripper_pos, double time)
+{
+ static std::vector <double> target_angle;
+ target_angle.clear();
+ target_angle.push_back(j1);
+ target_angle.push_back(j2);
+ target_angle.push_back(j3);
+ target_angle.push_back(j4);
+ open_manipulator->makeJointTrajectory(target_angle, time);
+ open_manipulator->makeToolTrajectory("gripper", gripper_pos);
+}
+
+/*****************************************************************************
+** Initialize Demo
+*****************************************************************************/
+void initDemo()
+{
+ start_motion_flag = false;
+ motion_cnt[0] = 0;
+
+ pinMode(BDPIN_PUSH_SW_1, INPUT);
+ pinMode(BDPIN_PUSH_SW_2, INPUT);
+ pinMode(BDPIN_GPIO_1, OUTPUT);
+ digitalWrite(BDPIN_GPIO_1, LOW);
+ Serial.println("OpenManipulator Init Completed");
+}
+
+/*****************************************************************************
+** Play Demo Motion
+*****************************************************************************/
+void runDemo(OpenManipulator *open_manipulator)
+{
+ if(!start_motion_flag && !stop_motion_flag)
+ {
+ startMotion();
+ }
+ if(!start_motion_flag && stop_motion_flag) // Restart DEMO
+ {
+ if(digitalRead(BDPIN_PUSH_SW_1))
+ {
+ startMotion();
+ }
+ }
+ if(digitalRead(BDPIN_PUSH_SW_2) && !stop_motion_flag) // Stop DEMO
+ {
+ stopMotion(open_manipulator);
+ }
+
+ if (open_manipulator->getMovingState())
+ {
+ return;
+ }
+ else 
+ {
+ if (start_motion_flag)
+ {
+ switch(motion_cnt[0])
+ {
+ case 0:
+ moveJS(open_manipulator, 0.0, -1.0, 0.2, 0.8, 0.0, 2.0); 
+ motion_cnt[0] ++; 
+ break;
+ case 1:
+ moveJS(open_manipulator, -0.5, 0.0, 1.0, -1.0, 0.01, 2.0); 
+ motion_cnt[0] ++; 
+ break;
+ case 2:
+ moveJS(open_manipulator, -0.5, 0.1, 0.75, -0.85, 0.01, 2.0); 
+ motion_cnt[0] ++; 
+ break;
+ case 3:
+ open_manipulator->sleepTrajectory(2.0);
+ motion_cnt[0] ++; 
+ break;
+ case 4:
+ moveJS(open_manipulator, -0.5, -0.05, 1.05, -1.0, 0.0, 2.0); 
+ motion_cnt[0] = 0; 
+ break;
+ default:
+ motion_cnt[0] = 0;
+ break;
+ }
+ }
+ }
+}
+
+/*****************************************************************************
+** Play Teaching Motion
+*****************************************************************************/
+void runTeachingMotion(OpenManipulator *open_manipulator)
+{
+ if(!start_motion_flag && !stop_motion_flag) // Start Teach Motion
+ {
+ startMotion();
+ }
+ if(!start_motion_flag && stop_motion_flag) // Restart Teach Motion
+ {
+ if(digitalRead(BDPIN_PUSH_SW_1))
+ {
+ startMotion();
+ }
+ }
+ if(digitalRead(BDPIN_PUSH_SW_2) && !stop_motion_flag) // Stop Teach Motion
+ {
+ stopMotion(open_manipulator);
+ digitalWrite(BDPIN_LED_USER_1, HIGH); // Turn off USER1 LED
+ digitalWrite(BDPIN_LED_USER_2, HIGH); // Turn off USER2 LED
+ digitalWrite(BDPIN_LED_USER_3, HIGH); // Turn off USER3 LED
+ digitalWrite(BDPIN_LED_USER_4, HIGH); // Turn off USER4 LED
+ }
+
+ if (open_manipulator->getMovingState()) // Check if OpenMANIPULATOR is moving along the trajectory
+ {
+ // If OpenMANIPULATOR is still moving along the trajectory, then do not get the next Pose and return to main loop
+ // Digital Output HIGH(3.3V) on OpenCR GPIO pin #3 (http:https://emanual.robotis.com/docs/en/parts/controller/opencr10/#gpio)
+ digitalWrite(BDPIN_GPIO_1, HIGH);
+ return;
+ }
+ else
+ {
+ // Digital Output LOW(0V) on OpenCR GPIO pin #3 (http:https://emanual.robotis.com/docs/en/parts/controller/opencr10/#gpio)
+ digitalWrite(BDPIN_GPIO_1, LOW);
+ if (start_motion_flag)
+ {
+ if (motion_cnt[0] < motion_index)
+ {
+ uint8_t joint1_index = motion_cnt[0]++;
+ uint8_t joint2_index = motion_cnt[0]++;
+ uint8_t joint3_index = motion_cnt[0]++;
+ uint8_t joint4_index = motion_cnt[0]++;
+ uint8_t gripper_index = motion_cnt[0]++;
+
+ // Play each motion during 3.0 seconds
+ 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);
+ }
+ else
+ {
+ motion_cnt[0] = 0;
+ }
+ }
+ }
+}
+
+/*****************************************************************************
+** Start Motion
+*****************************************************************************/
+void startMotion()
+{
+ Serial.println("Press [SW2] to Stop Motion");
+ // Start the motion
+ start_motion_flag = true;
+ stop_motion_flag = false;
+
+ motion_cnt[0] = 0;
+}
+
+/*****************************************************************************
+** Stop Motion
+*****************************************************************************/
+void stopMotion(OpenManipulator *open_manipulator)
+{
+ Serial.println("Press [SW1] to Start Motion");
+ // Stop the motion
+ start_motion_flag = false;
+ stop_motion_flag = true;
+
+ // Go to the Init Pose(Right angle pose).
+ moveJS(open_manipulator, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); 
+ motion_cnt[0] = 0;
+}

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

@@ -44,6 +44,7 @@ class OpenManipulator : public robotis_manipulator::RobotisManipulator
  robotis_manipulator::JointActuator *joint_;
  robotis_manipulator::ToolActuator *tool_;
  robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE];
+ uint8_t joint1_id, joint2_id, joint3_id, joint4_id, gripper_id;
 
 public:
  OpenManipulator();
@@ -54,6 +55,9 @@ class OpenManipulator : public robotis_manipulator::RobotisManipulator
  STRING baud_rate = "1000000", 
  float control_loop_time = 0.010);
  void processOpenManipulator(double present_time);
+
+ // Allow to use a custom joint ID for OpenMANIPULATOR-X
+ void setOpenManipulatorCustomJointId(uint8_t joint1, uint8_t joint2, uint8_t joint3, uint8_t joint4, uint8_t gripper);
 };
 
 #endif // OPEN_MANIPULTOR_H_