DRのPS3での操作用
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 3:78b608ba0286
- Parent:
- 2:b728a6564520
- Child:
- 4:3abefa13c2e3
--- a/main.cpp Thu Jul 08 12:55:18 2021 +0000 +++ b/main.cpp Thu Jul 08 18:20:31 2021 +0000 @@ -22,7 +22,7 @@ DigitalOut myled1(LED1); 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_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計算 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) @@ -75,17 +75,17 @@ motor.setEquation(0.0523,0.0148,0.0459,-0.0341); //Excel"omega_controll"の図より - target_rad = -(M_PI/6); //目標角度をπ/6(rad)に(単位に注意) ////////////////////////// - target_deg = radToDeg(target_rad); +// target_rad = -(M_PI/6); //目標角度をπ/6(rad)に(単位に注意) ////////////////////////// +// target_deg = radToDeg(target_rad); while (1) { //// ROS //// nh.spinOnce(); if(arm_num==1&&arm_deg==30) { //LEDは動作確認用 myled2 = !myled2; - myled3 = !myled3; arm_rad = degToRad(arm_deg); -// target_rad = arm_rad; + target_rad = -30; + target_deg = radToDeg(target_rad); } ////タッチセンサ//// @@ -95,16 +95,24 @@ servo_c.set_degree(0,servoDeg(10000)); //servo_c掴む hand = 1; } + } else { + hand = 0; } ////アブソリュートエンコーダ//// if(timer.read()>DELTA_T) { if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す - myled1=1; //LED2 点灯 - motor.AcOmega(target_rad); //速度式角度制御 + myled1=1; //LED1 点灯 + if(hand == 1) { + myled4=1; //LED4 点灯 + motor.AcOmega(target_rad); //速度式角度制御 + } else if(hand == 0) { + myled4=0; + } } else { //目標角度に達したら停止 - myled1=0; //LED2 消灯 + myled1=0; //LED1 消灯 motor.stop(); + myled4=0; //LED4 点灯 //wait(100); //table停止用時間 servo_c.set_degree(0,servoDeg(7500)); //servo_c離す }