DRのPS3での操作用
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
main.cpp
- Committer:
- ayu13
- Date:
- 2021-07-08
- Revision:
- 1:c0a3e4589a8f
- Parent:
- 0:83b2c6a67cce
- Child:
- 2:b728a6564520
File content as of revision 1:c0a3e4589a8f:
////c = center arm #include "mbed.h" #include "AMT21.h" #include "CalPID.h" #include "KondoServo.h" #include "MotorController.h" #include <stdlib.h> ///////////////// ROS ///////////////// #include <ros.h> #include <std_msgs/String.h> #include <geometry_msgs/Point.h> DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); float arm_num=0,arm_rad=0; void cArmCallback(const geometry_msgs::Point &arm_state) { arm_num = arm_state.x; //右回転=0、左回転=1 arm_rad = arm_state.y; //目標角度 } ros::NodeHandle nh; ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); ///////////////// 宣言部分 ////////////////////// #ifndef M_PI #define M_PI 3.14159265359f #endif #define DELTA_T 0.01 //制御周期 #define DUTY_MAX 0.07 //duty比の最大値 #define OMEGA_MAX 0.6 //速度制御を利用した角度制御での角速度の最大値 #define NUM_DATA 270 ////角度制御(アブソリュートエンコーダ)///// 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.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 target_speed=0; double target_rad=0; double target_deg = 0; double radToDeg(double x); ////先端アーム(サーボ)///// DigitalIn ts_c1(p6, PullUp); 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 angle_count=0; int omega_count=0; double now_deg=0; //度数法 double now_count=0; int main() { myled1 = 0; //アブソリュートエンコーダ用if文中で点灯 ///////// ROS ///////// nh.getHardware()->setBaud(921600); nh.initNode(); nh.subscribe(sub); timer.reset(); timer.start(); 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); while (1) { //// ROS //// nh.spinOnce(); target_rad = arm_rad; if(arm_num==1&&arm_rad==-(M_PI/6)) { //LEDは動作確認用 myled2 = !myled2; } ////タッチセンサ//// if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む wait_us(50); if(ts_c1 == 0) { servo_c.set_degree(0,servoDeg(10000)); //servo_c掴む hand = 1; } } ////アブソリュートエンコーダ//// if(timer.read()>DELTA_T) { if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す myled1=!myled1; //LED2 点灯 motor.AcOmega(target_rad); //速度式角度制御 } else { //目標角度に達したら停止 myled1=0; //LED2 消灯 motor.stop(); servo_c.set_degree(0,servoDeg(7500)); //servo_c離す } amt.rewriteCount(); amt.calOmega(); amt.getOmega(); now_deg = amt.getDeg(); //度数法 now_count=amt.getCount(); motor.AcOmega(target_rad); // pc.printf("now_deg = %f,%f\r\n", now_deg,now_count); //現在角度取得 (確認以外ではコメントアウトしないとモーター回らない)//////////////////// timer.reset(); } wait_us(10); } } ///////////////// 関数宣言部分 ////////////////////// double servoDeg(double servo_value) { return (servo_value - 3500)*270/(11500 - 3500); } double radToDeg(double x) { return x*180/M_PI; }