![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用(アームの開閉をコントローラで制御可能)
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 4:3abefa13c2e3
- Parent:
- 3:78b608ba0286
- Child:
- 5:4fba3e91741f
diff -r 78b608ba0286 -r 3abefa13c2e3 main.cpp --- a/main.cpp Thu Jul 08 18:20:31 2021 +0000 +++ b/main.cpp Sat Jul 10 06:19:53 2021 +0000 @@ -37,16 +37,15 @@ DigitalIn ts_c2(p7, PullUp); KondoServo servo_c(p13, p14); double servoDeg(double servo_value); -double calServoDeg(double servo_value); //360°の度数法にする int id_c1 = 0; float deg_c_grab = 10000; float deg_c_open = 7500; -int hand = 0; +int hand = 0; //タッチセンサがONになった状態:1,OFF:0 ////データ記録まわり///// int angle_count=0; int omega_count=0; -double now_deg=0; //度数法 +double now_deg=0; double now_count=0; ///////////////// ROS ///////////////// @@ -81,49 +80,46 @@ while (1) { //// ROS //// nh.spinOnce(); - if(arm_num==1&&arm_deg==30) { //LEDは動作確認用 - myled2 = !myled2; - arm_rad = degToRad(arm_deg); - target_rad = -30; - target_deg = radToDeg(target_rad); + 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(10000)); //servo_c掴む + servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む hand = 1; } - } else { - hand = 0; } ////アブソリュートエンコーダ//// if(timer.read()>DELTA_T) { if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す - myled1=1; //LED1 点灯 - if(hand == 1) { - myled4=1; //LED4 点灯 + myled1 = 1; //LED1 点灯 + if(hand == 1) { //タッチセンサが反応していたとき(ハンドルを掴んでいたとき) motor.AcOmega(target_rad); //速度式角度制御 } else if(hand == 0) { - myled4=0; + myled4=1; //LED4 点灯 } } else { //目標角度に達したら停止 myled1=0; //LED1 消灯 motor.stop(); - myled4=0; //LED4 点灯 + hand = 0; //wait(100); //table停止用時間 - servo_c.set_degree(0,servoDeg(7500)); //servo_c離す + servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す } amt.rewriteCount(); amt.calOmega(); amt.getOmega(); - now_deg = amt.getDeg(); //度数法 + now_deg = amt.getDeg(); now_count=amt.getCount(); - motor.AcOmega(target_rad); +// motor.AcOmega(target_rad); -// pc.printf("now_deg = %f,%f\r\n", now_deg,now_count); //現在角度取得 (確認以外ではコメントアウトしないとモーター回らない)//////////////////// timer.reset(); } wait_us(10);