DRのPS3での操作用
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 6:e6b303ea6439
- Parent:
- 5:4fba3e91741f
- Child:
- 7:038ea29c5a97
--- a/main.cpp Sat Jul 10 09:19:49 2021 +0000 +++ b/main.cpp Mon Jul 19 13:20:31 2021 +0000 @@ -15,7 +15,7 @@ #endif #define DELTA_T 0.01 //制御周期 #define DUTY_MAX 0.07 //duty比の最大値 -#define OMEGA_MAX 0.6 //速度制御を利用した角度制御での角速度の最大値 +#define OMEGA_MAX 0.25 //速度制御を利用した角度制御での角速度の最大値 #define NUM_DATA 270 int state = 0; @@ -25,7 +25,7 @@ Timer timer; CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX); //速度制御のPD計算 CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 -CalPID angle_omega_pid(5.5,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 +CalPID angle_omega_pid(5.0,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); @@ -43,8 +43,8 @@ double servoDeg(double servo_value); void touchGrab(); int id_c1 = 0; -float deg_c_grab = 10000; -float deg_c_open = 7500; +float deg_c_grab = 10600; +float deg_c_open = 8300; int hand = 0; //タッチセンサがONになった状態:1,OFF:0 ////データ記録まわり///// @@ -66,6 +66,9 @@ ros::NodeHandle nh; ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); +DigitalIn toggle1(p16, PullUp); //right +DigitalIn toggle2(p20, PullUp); //left + int main() { myled1 = 0; //目標角度以上離れている:点灯 @@ -95,16 +98,26 @@ myled2 = 1; } arm_rad = degToRad(arm_deg); -// target_rad = -arm_rad; -// target_deg = -arm_deg; - arm_deg = -30; +// target_rad = -arm_rad; +// target_deg = -arm_deg; + arm_deg = -15; arm_rad = degToRad(arm_deg); if(state == 0) { - target_deg = -60; + ///トグルスイッチ1////1 + if(toggle1 == 0) { //トグルスイッチ1(mbed側)ONで初start + wait_us(50); + if(toggle1 == 0) { + state ++; + } + } + } + if(state == 1) { + target_deg = -90; target_rad = degToRad(target_deg); + servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す state++; - } else if (state == 1) { + } else if (state == 2) { myled3 = 1; if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す myled1 = 1; //LED1 点灯 @@ -114,12 +127,12 @@ motor.stop(); } touchGrab(); //タッチセンサ - } else if (state == 2) { + } else if (state == 3) { myled3 = 0; target_rad = arm_rad; target_deg = arm_deg; state++; - } else if (state == 3) { + } else if (state == 4) { if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す myled1 = 1; //LED1 点灯 motor.AcOmega(target_rad); //速度式角度制御 @@ -129,6 +142,7 @@ //wait(100); //table停止用時間 servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す myled4 = 0; +// state = 0; } }