![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用(アームの開閉をコントローラで制御可能)
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 12:c2bb0a9587b5
- Parent:
- 11:df2acf22123e
--- a/main.cpp Sun Jul 25 13:57:43 2021 +0000 +++ b/main.cpp Fri Jul 30 11:36:46 2021 +0000 @@ -48,6 +48,8 @@ double now_deg=0; double now_count=0; +int catch_mode = 0; + ///////////////// ROS ///////////////// DigitalOut myled1(LED1); DigitalOut myled2(LED2); @@ -58,7 +60,9 @@ { arm_turn = arm_state.x; //左回転=1、右回転=2 arm_deg = arm_state.y; //目標角度 + catch_mode = arm_state.z; } + ros::NodeHandle nh; ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); @@ -86,6 +90,15 @@ while (1) { nh.spinOnce(); //ROS + + if(catch_mode == 1){ + servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す + myled1 = 1; + } + else if(catch_mode == 2){ + servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む + myled2 = 1; + } if(timer.read()>DELTA_T) { if(arm_turn==2&&arm_deg==-10) { //動作確認(通信)用 @@ -125,7 +138,8 @@ myled1=0; //LED1 消灯 motor.stop(); } - touchGrab(); + state++; + //touchGrab(); } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ. myled3 = 0; if(mode == 0){ @@ -141,7 +155,7 @@ target_rad = degToRad(target_deg); } state++; - } else if (state == 5) {//目標角度に近づいたら停止してサーボを離す. + } else if (state == 5) {//目標角度に近づいたら停止する if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す myled1 = 1; //LED1 点灯 motor.AcOmega(target_rad); //速度式角度制御