![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
Diff: main.cpp
- Revision:
- 9:95f6b9f54395
- Parent:
- 8:9656790eea17
- Child:
- 10:c1ca3db7177c
--- a/main.cpp Tue Jul 20 06:41:29 2021 +0000 +++ b/main.cpp Tue Jul 20 08:57:36 2021 +0000 @@ -21,39 +21,35 @@ int state = 0; ////角度制御(アブソリュートエンコーダ)///// -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_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); double degToRad(double x); -double target_speed=0; double target_rad=0; double target_deg = 0; double turn_deg = 0; double turn_rad = 0; +DigitalIn toggle1(p16, PullUp); //トグルright(マイコン側) +DigitalIn toggle2(p20, PullUp); //トグルleft(外側) ////先端アーム(サーボ)///// 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; +void touchGrab(); //タッチセンサ float deg_c_grab = 10600; float deg_c_open = 8300; -int hand = 0; //タッチセンサがONになった状態:1,OFF:0 -////データ記録まわり///// -int angle_count=0; -int omega_count=0; +////データ記録関連///// double now_deg=0; double now_count=0; ///////////////// ROS ///////////////// +DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); @@ -66,9 +62,6 @@ 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; //目標角度以上離れている:点灯 @@ -159,10 +152,6 @@ { return (servo_value - 3500)*270/(11500 - 3500); } -double radToDeg(double x) -{ - return x*180/M_PI; -} double degToRad(double x) { return x*M_PI/180;