![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用(アームの開閉をコントローラで制御可能)
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 1:c0a3e4589a8f
- Parent:
- 0:83b2c6a67cce
- Child:
- 2:b728a6564520
--- a/main.cpp Thu Jul 08 03:46:29 2021 +0000 +++ b/main.cpp Thu Jul 08 11:48:58 2021 +0000 @@ -13,11 +13,11 @@ DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); -float arm_num=0,arm_deg=0; +float arm_num=0,arm_rad=0; void cArmCallback(const geometry_msgs::Point &arm_state) { - arm_num = arm_state.x; - arm_deg = arm_state.y; + arm_num = arm_state.x; //右回転=0、左回転=1 + arm_rad = arm_state.y; //目標角度 } ros::NodeHandle nh; ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); @@ -26,12 +26,11 @@ #ifndef M_PI #define M_PI 3.14159265359f #endif -#define DELTA_T 0.01 //制御周期 -#define DUTY_MAX 0.07 //duty比の最大値 -#define OMEGA_MAX 0.6 //速度制御を利用した角度制御での角速度の最大値 +#define DELTA_T 0.01 //制御周期 +#define DUTY_MAX 0.07 //duty比の最大値 +#define OMEGA_MAX 0.6 //速度制御を利用した角度制御での角速度の最大値 #define NUM_DATA 270 -Serial pc(USBTX, USBRX); ////角度制御(アブソリュートエンコーダ)///// DigitalOut myled1(LED1); Timer timer; @@ -52,8 +51,9 @@ double servoDeg(double servo_value); double calServoDeg(double servo_value); //360°の度数法にする int id_c1 = 0; -float deg_c_grab = 9000; -float deg_c_open = 3500; +float deg_c_grab = 10000; +float deg_c_open = 7500; +int hand = 0; ////データ記録まわり///// @@ -66,7 +66,7 @@ { myled1 = 0; //アブソリュートエンコーダ用if文中で点灯 ///////// ROS ///////// - nh.getHardware()->setBaud(115200); + nh.getHardware()->setBaud(921600); nh.initNode(); nh.subscribe(sub); @@ -81,33 +81,29 @@ while (1) { //// ROS //// nh.spinOnce(); - if(arm_num==1&&arm_deg==60) { //LEDは動作確認用 + target_rad = arm_rad; + if(arm_num==1&&arm_rad==-(M_PI/6)) { //LEDは動作確認用 myled2 = !myled2; } - if(arm_num == 1) { - myled3 = 1; - } - if(arm_deg == 60) { - myled4 = 1; - } - + ////タッチセンサ//// if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む wait_us(50); if(ts_c1 == 0) { - servo_c.set_degree(0,servoDeg(4500)); //servo_c掴む + servo_c.set_degree(0,servoDeg(10000)); //servo_c掴む + hand = 1; } } ////アブソリュートエンコーダ//// if(timer.read()>DELTA_T) { if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す - myled1=1; //LED2 点灯 + myled1=!myled1; //LED2 点灯 motor.AcOmega(target_rad); //速度式角度制御 } else { //目標角度に達したら停止 myled1=0; //LED2 消灯 motor.stop(); - servo_c.set_degree(0,servoDeg(1000)); //servo_c離す + servo_c.set_degree(0,servoDeg(7500)); //servo_c離す } amt.rewriteCount(); amt.calOmega();