![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用(アームの開閉をコントローラで制御可能)
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 5:4fba3e91741f
- Parent:
- 4:3abefa13c2e3
- Child:
- 6:e6b303ea6439
--- a/main.cpp Sat Jul 10 06:19:53 2021 +0000 +++ b/main.cpp Sat Jul 10 09:19:49 2021 +0000 @@ -18,6 +18,8 @@ #define OMEGA_MAX 0.6 //速度制御を利用した角度制御での角速度の最大値 #define NUM_DATA 270 +int state = 0; + ////角度制御(アブソリュートエンコーダ)///// DigitalOut myled1(LED1); Timer timer; @@ -26,17 +28,20 @@ CalPID angle_omega_pid(5.5,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 Amt21 amt(p9,p10,p8); //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode) MotorController motor(p26,p25,DELTA_T,amt,speed_pid,angle_duty_pid,angle_omega_pid); //MotorController(PinName motor_p_, PinName motor_n_,double dt, Amt21 &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega) +double radToDeg(double x); +double degToRad(double x); double target_speed=0; double target_rad=0; double target_deg = 0; -double radToDeg(double x); -double degToRad(double x); +double right_deg = -90; +double left_deg = 90; ////先端アーム(サーボ)///// DigitalIn ts_c1(p6, PullUp); DigitalIn ts_c2(p7, PullUp); KondoServo servo_c(p13, p14); double servoDeg(double servo_value); +void touchGrab(); int id_c1 = 0; float deg_c_grab = 10000; float deg_c_open = 7500; @@ -63,7 +68,11 @@ int main() { - myled1 = 0; //アブソリュートエンコーダ用if文中で点灯 + myled1 = 0; //目標角度以上離れている:点灯 + myled2 = 0; //通信で欲しい値が送られているとき:点灯 + myled3 = 0; // + myled4 = 0; //サーボを掴む→点灯.離す→消灯 + ///////// ROS ///////// nh.getHardware()->setBaud(921600); nh.initNode(); @@ -80,39 +89,68 @@ while (1) { //// ROS //// nh.spinOnce(); - if(arm_num==1&&arm_deg==30) { //動作確認用 - myled2 = 1; - } - arm_rad = degToRad(arm_deg); - target_rad = -arm_rad; - target_deg = -arm_deg; - - ////タッチセンサ//// - if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む - wait_us(50); - if(ts_c1 == 0) { - servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む - hand = 1; + if(timer.read()>DELTA_T) { + if(arm_num==1&&arm_deg==30) { //動作確認用 + myled2 = 1; } - } + arm_rad = degToRad(arm_deg); +// target_rad = -arm_rad; +// target_deg = -arm_deg; + arm_deg = -30; + arm_rad = degToRad(arm_deg); - ////アブソリュートエンコーダ//// - if(timer.read()>DELTA_T) { - if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す - myled1 = 1; //LED1 点灯 - if(hand == 1) { //タッチセンサが反応していたとき(ハンドルを掴んでいたとき) + if(state == 0) { + target_deg = -60; + target_rad = degToRad(target_deg); + state++; + } else if (state == 1) { + myled3 = 1; + if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す + myled1 = 1; //LED1 点灯 + motor.AcOmega(target_rad); //速度式角度制御 + } else { //目標角度に達したら停止 + myled1=0; //LED1 消灯 + motor.stop(); + } + touchGrab(); //タッチセンサ + } else if (state == 2) { + myled3 = 0; + target_rad = arm_rad; + target_deg = arm_deg; + state++; + } else if (state == 3) { + if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す + myled1 = 1; //LED1 点灯 motor.AcOmega(target_rad); //速度式角度制御 - } else if(hand == 0) { - myled4=1; //LED4 点灯 + } else { //目標角度に達したら停止 + myled1=0; //LED1 消灯 + motor.stop(); + //wait(100); //table停止用時間 + servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す + myled4 = 0; } - } else { //目標角度に達したら停止 - myled1=0; //LED1 消灯 - motor.stop(); - hand = 0; - //wait(100); //table停止用時間 - servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す } + + ////タッチセンサ//// +// touchGrab(); + + ////アブソリュートエンコーダ//// + +// if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す +// myled1 = 1; //LED1 点灯 +//// if(hand == 1) { //タッチセンサが反応していたとき(ハンドルを掴んでいたとき) +// motor.AcOmega(target_rad); //速度式角度制御 +//// } else if(hand == 0) { +//// myled4=0; //LED4 消灯 +//// } +// } else if(fabs(now_deg-target_deg) < 0.5 && state == 3) { //目標角度に達したら停止 +// myled1=0; //LED1 消灯 +// motor.stop(); +// hand = 0; +// //wait(100); //table停止用時間 +// servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す +// } amt.rewriteCount(); amt.calOmega(); amt.getOmega(); @@ -138,4 +176,16 @@ double degToRad(double x) { return x*M_PI/180; +} +void touchGrab() +{ + if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む + wait_us(50); + if(ts_c1 == 0) { + servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む + state++; +// hand = 1; + myled4=1; //LED4 点灯 + } + } } \ No newline at end of file