From 737b3ce5329503b958ac490eda44736ca83ec3ae Mon Sep 17 00:00:00 2001 From: "LAPTOP-HS5NB06S\\29019" <290198252@qq.com> Date: Sun, 31 May 2020 21:16:15 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86moveoros=E4=BB=BF?= =?UTF-8?q?=E7=9C=9F=E7=9A=84=E4=B8=8B=E4=BD=8D=E6=9C=BA=E6=B5=8B=E8=AF=95?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../moveo_moveit/move_moveit/move_moveit.ino | 154 ++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 arduino/arduino/moveo_moveit/move_moveit/move_moveit.ino diff --git a/arduino/arduino/moveo_moveit/move_moveit/move_moveit.ino b/arduino/arduino/moveo_moveit/move_moveit/move_moveit.ino new file mode 100644 index 0000000..7519372 --- /dev/null +++ b/arduino/arduino/moveo_moveit/move_moveit/move_moveit.ino @@ -0,0 +1,154 @@ +/* Purpose: This sketch uses ROS as well as MultiStepper, AccelStepper, and Servo libraries to control the + * BCN3D Moveo robotic arm. In this setup, a Ramps 1.4 shield is used on top of an Arduino Mega 2560. + * Subscribing to the following ROS topics: 1) joint_steps, 2) gripper_angle + * 1) joint_steps is computed from the simulation in PC and sent Arduino via rosserial. It contains + * the steps (relative to the starting position) necessary for each motor to move to reach the goal position. + * 2) gripper_angle contains the necessary gripper angle to grasp the object when the goal state is reached + * + * Publishing to the following ROS topics: joint_steps_feedback + * 1) joint_steps_feedback is a topic used for debugging to make sure the Arduino is receiving the joint_steps data + * accurately + * + * Author: Jesse Weisberg + */ +#if (ARDUINO >= 100) + #include +#else + #include +#endif +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Joint 1 +#define E1_STEP_PIN 53 +#define E1_DIR_PIN 51 +#define E1_ENABLE_PIN 31 + +// Joint 2 +#define Z_STEP_PIN 32 +#define Z_DIR_PIN 30 +#define Z_ENABLE_PIN 62 +#define Z_MIN_PIN 18 +#define Z_MAX_PIN 19 + +// Joint 3 +#define Y_STEP_PIN 48 +#define Y_DIR_PIN 46 +#define Y_ENABLE_PIN 56 +#define Y_MIN_PIN 14 +#define Y_MAX_PIN 15 + +// Joint 4 +#define X_STEP_PIN 40 +#define X_DIR_PIN 42 +#define X_ENABLE_PIN 38 + +// Joint 5 +#define E0_STEP_PIN 43 +#define E0_DIR_PIN 41 +#define E0_ENABLE_PIN 24 + + +AccelStepper joint1(1,E1_STEP_PIN, E1_DIR_PIN); +AccelStepper joint2(1,Z_STEP_PIN, Z_DIR_PIN); +AccelStepper joint3(1,Y_STEP_PIN, Y_DIR_PIN); +AccelStepper joint4(1,X_STEP_PIN, X_DIR_PIN); +AccelStepper joint5(1, E0_STEP_PIN, E0_DIR_PIN); + +Servo gripper; +MultiStepper steppers; + +int joint_step[6]; +int joint_status = 0; + +ros::NodeHandle nh; +std_msgs::Int16 msg; + +//instantiate publisher (for debugging purposes) +//ros::Publisher steps("joint_steps_feedback",&msg); + +void arm_cb(const moveo_moveit::ArmJointState& arm_steps){ + joint_status = 1; + joint_step[0] = arm_steps.position1; + joint_step[1] = arm_steps.position2; + joint_step[2] = arm_steps.position3; + joint_step[3] = arm_steps.position4; + joint_step[4] = arm_steps.position5; + joint_step[5] = arm_steps.position6; //gripper position <0-180> +} + +void gripper_cb( const std_msgs::UInt16& cmd_msg){ + gripper.write(cmd_msg.data); // Set servo angle, should be from 0-180 + digitalWrite(13, HIGH-digitalRead(13)); // Toggle led +} + +//instantiate subscribers +ros::Subscriber arm_sub("joint_steps",arm_cb); //subscribes to joint_steps on arm +ros::Subscriber gripper_sub("gripper_angle", gripper_cb); //subscribes to gripper position +//to publish from terminal: rostopic pub gripper_angle std_msgs/UInt16 <0-180> + +void setup() { + //put your setup code here, to run once: + //Serial.begin(57600); + pinMode(13,OUTPUT); + joint_status = 1; + + nh.initNode(); + nh.subscribe(arm_sub); + nh.subscribe(gripper_sub); + //nh.advertise(steps); + + // Configure each stepper + joint1.setMaxSpeed(1500); + joint2.setMaxSpeed(750); + joint3.setMaxSpeed(2000); + joint4.setMaxSpeed(500); + joint5.setMaxSpeed(1000); + + // Then give them to MultiStepper to manage + steppers.addStepper(joint1); + steppers.addStepper(joint2); + steppers.addStepper(joint3); + steppers.addStepper(joint4); + steppers.addStepper(joint5); + + // Configure gripper servo + gripper.attach(11); + + digitalWrite(13, 1); //toggle led +} + +void loop() { + if (joint_status == 1) // If command callback (arm_cb) is being called, execute stepper command + { + long positions[5]; // Array of desired stepper positions must be long + positions[0] = joint_step[0]; // negated since the real robot rotates in the opposite direction as ROS + positions[1] = -joint_step[1]; + positions[2] = joint_step[2]; + positions[3] = joint_step[3]; + positions[4] = -joint_step[4]; + + // Publish back to ros to check if everything's correct + //msg.data=positions[4]; + //steps.publish(&msg); + + steppers.moveTo(positions); + nh.spinOnce(); + steppers.runSpeedToPosition(); // Blocks until all are in position + gripper.write(joint_step[5]); // move gripper after manipulator reaches goal + } + digitalWrite(13, HIGH-digitalRead(13)); //toggle led + joint_status = 0; + + nh.spinOnce(); + delay(1); +}