![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 11:df2acf22123e
- Parent:
- 10:c1ca3db7177c
--- a/main.cpp Tue Jul 20 12:02:23 2021 +0000 +++ b/main.cpp Sun Jul 25 13:57:43 2021 +0000 @@ -18,7 +18,7 @@ #define OMEGA_MAX 0.25 //速度制御を利用した角度制御での角速度の最大値 #define NUM_DATA 270 -int state = 0; +int state = 1; ////角度制御(アブソリュートエンコーダ)///// Timer timer; @@ -41,7 +41,7 @@ KondoServo servo_c(p13, p14); double servoDeg(double servo_value); void touchGrab(); //タッチセンサ反応したらサーボ掴む -float deg_c_grab = 10600; +float deg_c_grab = 11000; float deg_c_open = 8300; ////データ記録関連///// @@ -64,13 +64,14 @@ int main() { + int mode = 0; myled1 = 0; //目標角度以上離れている:点灯 myled2 = 0; //通信で欲しい値が送られているとき:点灯 myled3 = 0; //state2に入ったとき←基本不要 myled4 = 0; //サーボを掴む→点灯.離す→消灯 ///////// ROS ///////// - nh.getHardware()->setBaud(921600); + nh.getHardware()->setBaud(115200); nh.initNode(); nh.subscribe(sub); @@ -78,6 +79,10 @@ timer.start(); motor.setEquation(0.0523,0.0148,0.0459,-0.0341); //Excel"omega_controll"の図より + //////////////////////////////////////// + /*arm_turn = 2; //左回転=1、右回転=2 + arm_deg = -90; //目標角度*/ +/////////////////////////////////////// while (1) { nh.spinOnce(); //ROS @@ -123,8 +128,18 @@ touchGrab(); } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ. myled3 = 0; - target_rad = arm_rad; - target_deg = arm_deg; + if(mode == 0){ + target_rad = arm_rad; + target_deg = arm_deg; + } + else if(mode == 1){ + target_deg = 45; + target_rad = degToRad(target_deg); + } + else if(mode == 2){ + target_deg = -45; + target_rad = degToRad(target_deg); + } state++; } else if (state == 5) {//目標角度に近づいたら停止してサーボを離す. if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す @@ -133,9 +148,15 @@ } else { //目標角度に達したら停止 myled1 = 0; //LED1 消灯 motor.stop(); - servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す + //servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す myled4 = 0; -// state = 0; /////////////////////連続して動かす時はここのコメントアウト外す. + if(mode == 0){ + mode = 1; + } + else{ + mode = 3-mode; + } + state = 4; /////////////////////連続して動かす時はここのコメントアウト外す. } }