catchrobo2022 / Mbed 2 deprecated catchrobo2022_mbed

catchrobo2022 mbed LPC1768 メインプログラム

Dependencies:   mbed

main.cpp@0:803105042c95, 2022-09-26 (annotated)

Committer:
shimizuta
Date:
Mon Sep 26 13:45:05 2022 +0000
Revision:
0:803105042c95
catchrobo2022 finish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 0:803105042c95 1 #define USE_MBED
shimizuta 0:803105042c95 2
shimizuta 0:803105042c95 3 #ifdef USE_MBED
shimizuta 0:803105042c95 4 #include "mbed.h"
shimizuta 0:803105042c95 5 #include "motor_driver_bridge/motor_driver_bridge_mbed.h"
shimizuta 0:803105042c95 6 #include "catchrobo_sim/ros_bridge_mbed.h"
shimizuta 0:803105042c95 7 const float MBED2ROS_DT = 0.1; // 10Hz
shimizuta 0:803105042c95 8 #else
shimizuta 0:803105042c95 9 #include <ros/ros.h>
shimizuta 0:803105042c95 10 #include "sim_only/mbed.h"
shimizuta 0:803105042c95 11 #include "sim_only/motor_driver_bridge_sim.h"
shimizuta 0:803105042c95 12 #include "sim_only/ros_bridge_sim.h"
shimizuta 0:803105042c95 13 #include "sim_only/ticker_sim.h"
shimizuta 0:803105042c95 14 const float MBED2ROS_DT = 0.01;
shimizuta 0:803105042c95 15 #endif
shimizuta 0:803105042c95 16
shimizuta 0:803105042c95 17 #include <std_msgs/Float32MultiArray.h>
shimizuta 0:803105042c95 18 #include "catchrobo_sim/robot_manager.h"
shimizuta 0:803105042c95 19 #include "catchrobo_sim/gripper_manager.h"
shimizuta 0:803105042c95 20
shimizuta 0:803105042c95 21 // const float SPIN_FREQUENCY_s = 0.001;
shimizuta 0:803105042c95 22 const float MBED2GRIPPER_DT = 0.1;
shimizuta 0:803105042c95 23 const float MBED2MOTOR_DT = 0.005; // 200Hz
shimizuta 0:803105042c95 24 const int SERIAL_BAUD_RATE = 115200;
shimizuta 0:803105042c95 25 const float ARRIVE_THRESHOLD_RAD[] = {0.1, 0.1, 0.1};
shimizuta 0:803105042c95 26 const float FRICTION[] = {0.2, 0, 0};
shimizuta 0:803105042c95 27 const float GRIPPER_THRESHOLD_RAD = 0.1;
shimizuta 0:803105042c95 28 const float ESTIMATE_ERROR_LIMIT_RAD = 0.5;
shimizuta 0:803105042c95 29
shimizuta 0:803105042c95 30 MotorDriverBridge motor_driver_bridge;
shimizuta 0:803105042c95 31 RosBridge ros_bridge;
shimizuta 0:803105042c95 32 RobotManager robot_manager;
shimizuta 0:803105042c95 33 GripperManager gripper_manager;
shimizuta 0:803105042c95 34 EnableManager enable_manager;
shimizuta 0:803105042c95 35
shimizuta 0:803105042c95 36 void enableAll(bool is_enable)
shimizuta 0:803105042c95 37 {
shimizuta 0:803105042c95 38 for (int i = 0; i < N_MOTORS; i++)
shimizuta 0:803105042c95 39 {
shimizuta 0:803105042c95 40 motor_driver_bridge.enableMotor(i, is_enable);
shimizuta 0:803105042c95 41 wait(0.1);
shimizuta 0:803105042c95 42 }
shimizuta 0:803105042c95 43 }
shimizuta 0:803105042c95 44
shimizuta 0:803105042c95 45 void motorDriverCallback(const StateStruct &input)
shimizuta 0:803105042c95 46 {
shimizuta 0:803105042c95 47 robot_manager.setCurrentState(input);
shimizuta 0:803105042c95 48 };
shimizuta 0:803105042c95 49
shimizuta 0:803105042c95 50 void rosCallback(const catchrobo_msgs::MyRosCmd &command)
shimizuta 0:803105042c95 51 {
shimizuta 0:803105042c95 52 if (command.mode == catchrobo_msgs::MyRosCmd::PEG_IN_HOLE_MODE)
shimizuta 0:803105042c95 53 {
shimizuta 0:803105042c95 54 bool result[N_MOTORS] = {};
shimizuta 0:803105042c95 55 robot_manager.arriveCheck(result);
shimizuta 0:803105042c95 56 for (int i = 0; i < N_MOTORS; i++)
shimizuta 0:803105042c95 57 {
shimizuta 0:803105042c95 58 if (result[i])
shimizuta 0:803105042c95 59 {
shimizuta 0:803105042c95 60 catchrobo_msgs::ErrorCode error;
shimizuta 0:803105042c95 61 error.id = i;
shimizuta 0:803105042c95 62 error.error_code = catchrobo_msgs::ErrorCode::FINISH;
shimizuta 0:803105042c95 63 ros_bridge.publishError(error);
shimizuta 0:803105042c95 64 // ros_bridge.publishFinishFlag(i);
shimizuta 0:803105042c95 65 // ros::Duration dt = ros::Time::now() - t;
shimizuta 0:803105042c95 66 // ROS_INFO_STREAM("dt " << dt.toSec() );
shimizuta 0:803105042c95 67 }
shimizuta 0:803105042c95 68 }
shimizuta 0:803105042c95 69 return;
shimizuta 0:803105042c95 70 }
shimizuta 0:803105042c95 71 robot_manager.setRosCmd(command);
shimizuta 0:803105042c95 72 gripper_manager.setRosCmd(command);
shimizuta 0:803105042c95 73 }
shimizuta 0:803105042c95 74
shimizuta 0:803105042c95 75 void mbed2MotorDriverTimerCallback()
shimizuta 0:803105042c95 76 {
shimizuta 0:803105042c95 77
shimizuta 0:803105042c95 78 // ros::Time t = ros::Time::now();
shimizuta 0:803105042c95 79 //// enable check
shimizuta 0:803105042c95 80 catchrobo_msgs::ErrorCode error;
shimizuta 0:803105042c95 81 sensor_msgs::JointState joint_state;
shimizuta 0:803105042c95 82 robot_manager.getJointState(joint_state);
shimizuta 0:803105042c95 83 enable_manager.check(joint_state, error);
shimizuta 0:803105042c95 84
shimizuta 0:803105042c95 85 //// errorならdisable指示およびerror のpublish
shimizuta 0:803105042c95 86 if (error.error_code != catchrobo_msgs::ErrorCode::NONE)
shimizuta 0:803105042c95 87 {
shimizuta 0:803105042c95 88 enableAll(false);
shimizuta 0:803105042c95 89 enable_manager.setCurrentEnable(false);
shimizuta 0:803105042c95 90 ros_bridge.publishError(error);
shimizuta 0:803105042c95 91 }
shimizuta 0:803105042c95 92
shimizuta 0:803105042c95 93 if (enable_manager.getEnable()) //// enableならtをすすめる
shimizuta 0:803105042c95 94 {
shimizuta 0:803105042c95 95 robot_manager.nextStep(MBED2MOTOR_DT);
shimizuta 0:803105042c95 96 }
shimizuta 0:803105042c95 97 else //// disableなら脱力指示
shimizuta 0:803105042c95 98 {
shimizuta 0:803105042c95 99 robot_manager.disable();
shimizuta 0:803105042c95 100 }
shimizuta 0:803105042c95 101 //// update target value
shimizuta 0:803105042c95 102 ControlStruct control[N_MOTORS] = {};
shimizuta 0:803105042c95 103 ControlResult::ControlResult result[N_MOTORS] = {};
shimizuta 0:803105042c95 104 robot_manager.getMotorDrivesCommand(control, result);
shimizuta 0:803105042c95 105 for (int i = 0; i < N_MOTORS; i++)
shimizuta 0:803105042c95 106 {
shimizuta 0:803105042c95 107 motor_driver_bridge.publish(control[i]);
shimizuta 0:803105042c95 108 if (result[i] == ControlResult::FINISH)
shimizuta 0:803105042c95 109 {
shimizuta 0:803105042c95 110 error.id = i;
shimizuta 0:803105042c95 111 error.error_code = catchrobo_msgs::ErrorCode::FINISH;
shimizuta 0:803105042c95 112 ros_bridge.publishError(error);
shimizuta 0:803105042c95 113 // ros_bridge.publishFinishFlag(i);
shimizuta 0:803105042c95 114 // ros::Duration dt = ros::Time::now() - t;
shimizuta 0:803105042c95 115 // ROS_INFO_STREAM("dt " << dt.toSec() );
shimizuta 0:803105042c95 116 }
shimizuta 0:803105042c95 117 }
shimizuta 0:803105042c95 118 }
shimizuta 0:803105042c95 119
shimizuta 0:803105042c95 120 void mbed2RosTimerCallback()
shimizuta 0:803105042c95 121 {
shimizuta 0:803105042c95 122 //// radius
shimizuta 0:803105042c95 123 // sensor_msgs::JointState joint_state;
shimizuta 0:803105042c95 124 // robot_manager.getJointState(joint_state);
shimizuta 0:803105042c95 125
shimizuta 0:803105042c95 126 std_msgs::Float32MultiArray joint_rad;
shimizuta 0:803105042c95 127 robot_manager.getJointRad(joint_rad);
shimizuta 0:803105042c95 128 joint_rad.data[N_MOTORS] = gripper_manager.getJointRad();
shimizuta 0:803105042c95 129 ros_bridge.publishJointState(joint_rad);
shimizuta 0:803105042c95 130 };
shimizuta 0:803105042c95 131 void enableCallback(const catchrobo_msgs::EnableCmd &input)
shimizuta 0:803105042c95 132 {
shimizuta 0:803105042c95 133 enable_manager.setCmd(input);
shimizuta 0:803105042c95 134 enableAll(input.is_enable);
shimizuta 0:803105042c95 135 enable_manager.setCurrentEnable(input.is_enable);
shimizuta 0:803105042c95 136 }
shimizuta 0:803105042c95 137
shimizuta 0:803105042c95 138 void gripperTimerCallback()
shimizuta 0:803105042c95 139 {
shimizuta 0:803105042c95 140
shimizuta 0:803105042c95 141 ControlStruct control;
shimizuta 0:803105042c95 142 ControlResult::ControlResult result;
shimizuta 0:803105042c95 143 gripper_manager.getMotorDrivesCommand(control, result);
shimizuta 0:803105042c95 144 gripper_manager.nextStep(MBED2GRIPPER_DT);
shimizuta 0:803105042c95 145 motor_driver_bridge.publish(control);
shimizuta 0:803105042c95 146 // ROS_INFO_STREAM("gripper id " << control.id <<" p " << control.p_des);
shimizuta 0:803105042c95 147 // ROS_INFO_STREAM(control);
shimizuta 0:803105042c95 148 if (result == ControlResult::FINISH)
shimizuta 0:803105042c95 149 {
shimizuta 0:803105042c95 150 catchrobo_msgs::ErrorCode error;
shimizuta 0:803105042c95 151 error.id = N_MOTORS;
shimizuta 0:803105042c95 152 error.error_code = catchrobo_msgs::ErrorCode::FINISH;
shimizuta 0:803105042c95 153 ros_bridge.publishError(error);
shimizuta 0:803105042c95 154 // ros_bridge.publishFinishFlag(i);
shimizuta 0:803105042c95 155 }
shimizuta 0:803105042c95 156 }
shimizuta 0:803105042c95 157
shimizuta 0:803105042c95 158 int main(int argc, char **argv)
shimizuta 0:803105042c95 159 {
shimizuta 0:803105042c95 160 #ifndef USE_MBED
shimizuta 0:803105042c95 161 ros::init(argc, argv, "mbed_sim");
shimizuta 0:803105042c95 162 ros::NodeHandle nh("");
shimizuta 0:803105042c95 163 motor_driver_bridge.setNodeHandlePtr(&nh);
shimizuta 0:803105042c95 164 ros_bridge.setNodeHandlePtr(&nh);
shimizuta 0:803105042c95 165 #endif
shimizuta 0:803105042c95 166 robot_manager.init(ARRIVE_THRESHOLD_RAD, FRICTION, ESTIMATE_ERROR_LIMIT_RAD);
shimizuta 0:803105042c95 167 gripper_manager.init(GRIPPER_THRESHOLD_RAD, ESTIMATE_ERROR_LIMIT_RAD);
shimizuta 0:803105042c95 168
shimizuta 0:803105042c95 169 ros_bridge.init(SERIAL_BAUD_RATE, rosCallback, enableCallback);
shimizuta 0:803105042c95 170
shimizuta 0:803105042c95 171 int motor_directions[] = {-1, -1, -1, 1};
shimizuta 0:803105042c95 172 motor_driver_bridge.init(motorDriverCallback, motor_directions);
shimizuta 0:803105042c95 173
shimizuta 0:803105042c95 174 //// 初期値を暫定原点にする。後にROS指示で原点だしを行う
shimizuta 0:803105042c95 175 for (size_t i = 0; i < N_MOTORS; i++)
shimizuta 0:803105042c95 176 {
shimizuta 0:803105042c95 177 motor_driver_bridge.setOrigin(i);
shimizuta 0:803105042c95 178 wait(0.5);
shimizuta 0:803105042c95 179 }
shimizuta 0:803105042c95 180
shimizuta 0:803105042c95 181 //// motor driverへの指示開始
shimizuta 0:803105042c95 182 Ticker ticker_motor_driver_send;
shimizuta 0:803105042c95 183 ticker_motor_driver_send.attach(&mbed2MotorDriverTimerCallback, MBED2MOTOR_DT);
shimizuta 0:803105042c95 184
shimizuta 0:803105042c95 185 //// ros へのフィードバック開始
shimizuta 0:803105042c95 186 Ticker ticker;
shimizuta 0:803105042c95 187 ticker.attach(&mbed2RosTimerCallback, MBED2ROS_DT);
shimizuta 0:803105042c95 188
shimizuta 0:803105042c95 189 Ticker ticker_gripper;
shimizuta 0:803105042c95 190 ticker_gripper.attach(&gripperTimerCallback, MBED2GRIPPER_DT);
shimizuta 0:803105042c95 191
shimizuta 0:803105042c95 192 ros_bridge.spin();
shimizuta 0:803105042c95 193 }