![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用(アームの開閉をコントローラで制御可能)
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
main.cpp@1:c0a3e4589a8f, 2021-07-08 (annotated)
- Committer:
- ayu13
- Date:
- Thu Jul 08 11:48:58 2021 +0000
- Revision:
- 1:c0a3e4589a8f
- Parent:
- 0:83b2c6a67cce
- Child:
- 2:b728a6564520
okasii
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ayu13 | 0:83b2c6a67cce | 1 | ////c = center arm |
ayu13 | 0:83b2c6a67cce | 2 | #include "mbed.h" |
ayu13 | 0:83b2c6a67cce | 3 | #include "AMT21.h" |
ayu13 | 0:83b2c6a67cce | 4 | #include "CalPID.h" |
ayu13 | 0:83b2c6a67cce | 5 | #include "KondoServo.h" |
ayu13 | 0:83b2c6a67cce | 6 | #include "MotorController.h" |
ayu13 | 0:83b2c6a67cce | 7 | #include <stdlib.h> |
ayu13 | 0:83b2c6a67cce | 8 | |
ayu13 | 0:83b2c6a67cce | 9 | ///////////////// ROS ///////////////// |
ayu13 | 0:83b2c6a67cce | 10 | #include <ros.h> |
ayu13 | 0:83b2c6a67cce | 11 | #include <std_msgs/String.h> |
ayu13 | 0:83b2c6a67cce | 12 | #include <geometry_msgs/Point.h> |
ayu13 | 0:83b2c6a67cce | 13 | DigitalOut myled2(LED2); |
ayu13 | 0:83b2c6a67cce | 14 | DigitalOut myled3(LED3); |
ayu13 | 0:83b2c6a67cce | 15 | DigitalOut myled4(LED4); |
ayu13 | 1:c0a3e4589a8f | 16 | float arm_num=0,arm_rad=0; |
ayu13 | 0:83b2c6a67cce | 17 | void cArmCallback(const geometry_msgs::Point &arm_state) |
ayu13 | 0:83b2c6a67cce | 18 | { |
ayu13 | 1:c0a3e4589a8f | 19 | arm_num = arm_state.x; //右回転=0、左回転=1 |
ayu13 | 1:c0a3e4589a8f | 20 | arm_rad = arm_state.y; //目標角度 |
ayu13 | 0:83b2c6a67cce | 21 | } |
ayu13 | 0:83b2c6a67cce | 22 | ros::NodeHandle nh; |
ayu13 | 0:83b2c6a67cce | 23 | ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); |
ayu13 | 0:83b2c6a67cce | 24 | |
ayu13 | 0:83b2c6a67cce | 25 | ///////////////// 宣言部分 ////////////////////// |
ayu13 | 0:83b2c6a67cce | 26 | #ifndef M_PI |
ayu13 | 0:83b2c6a67cce | 27 | #define M_PI 3.14159265359f |
ayu13 | 0:83b2c6a67cce | 28 | #endif |
ayu13 | 1:c0a3e4589a8f | 29 | #define DELTA_T 0.01 //制御周期 |
ayu13 | 1:c0a3e4589a8f | 30 | #define DUTY_MAX 0.07 //duty比の最大値 |
ayu13 | 1:c0a3e4589a8f | 31 | #define OMEGA_MAX 0.6 //速度制御を利用した角度制御での角速度の最大値 |
ayu13 | 0:83b2c6a67cce | 32 | #define NUM_DATA 270 |
ayu13 | 0:83b2c6a67cce | 33 | |
ayu13 | 0:83b2c6a67cce | 34 | ////角度制御(アブソリュートエンコーダ)///// |
ayu13 | 0:83b2c6a67cce | 35 | DigitalOut myled1(LED1); |
ayu13 | 0:83b2c6a67cce | 36 | Timer timer; |
ayu13 | 0:83b2c6a67cce | 37 | CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX); //速度制御のPD計算 |
ayu13 | 0:83b2c6a67cce | 38 | CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 不要? |
ayu13 | 0:83b2c6a67cce | 39 | CalPID angle_omega_pid(5.5,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 |
ayu13 | 0:83b2c6a67cce | 40 | Amt21 amt(p9,p10,p8); //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode) |
ayu13 | 0:83b2c6a67cce | 41 | 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) |
ayu13 | 0:83b2c6a67cce | 42 | double target_speed=0; |
ayu13 | 0:83b2c6a67cce | 43 | double target_rad=0; |
ayu13 | 0:83b2c6a67cce | 44 | double target_deg = 0; |
ayu13 | 0:83b2c6a67cce | 45 | double radToDeg(double x); |
ayu13 | 0:83b2c6a67cce | 46 | |
ayu13 | 0:83b2c6a67cce | 47 | ////先端アーム(サーボ)///// |
ayu13 | 0:83b2c6a67cce | 48 | DigitalIn ts_c1(p6, PullUp); |
ayu13 | 0:83b2c6a67cce | 49 | DigitalIn ts_c2(p7, PullUp); |
ayu13 | 0:83b2c6a67cce | 50 | KondoServo servo_c(p13, p14); |
ayu13 | 0:83b2c6a67cce | 51 | double servoDeg(double servo_value); |
ayu13 | 0:83b2c6a67cce | 52 | double calServoDeg(double servo_value); //360°の度数法にする |
ayu13 | 0:83b2c6a67cce | 53 | int id_c1 = 0; |
ayu13 | 1:c0a3e4589a8f | 54 | float deg_c_grab = 10000; |
ayu13 | 1:c0a3e4589a8f | 55 | float deg_c_open = 7500; |
ayu13 | 1:c0a3e4589a8f | 56 | int hand = 0; |
ayu13 | 0:83b2c6a67cce | 57 | |
ayu13 | 0:83b2c6a67cce | 58 | |
ayu13 | 0:83b2c6a67cce | 59 | ////データ記録まわり///// |
ayu13 | 0:83b2c6a67cce | 60 | int angle_count=0; |
ayu13 | 0:83b2c6a67cce | 61 | int omega_count=0; |
ayu13 | 0:83b2c6a67cce | 62 | double now_deg=0; //度数法 |
ayu13 | 0:83b2c6a67cce | 63 | double now_count=0; |
ayu13 | 0:83b2c6a67cce | 64 | |
ayu13 | 0:83b2c6a67cce | 65 | int main() |
ayu13 | 0:83b2c6a67cce | 66 | { |
ayu13 | 0:83b2c6a67cce | 67 | myled1 = 0; //アブソリュートエンコーダ用if文中で点灯 |
ayu13 | 0:83b2c6a67cce | 68 | ///////// ROS ///////// |
ayu13 | 1:c0a3e4589a8f | 69 | nh.getHardware()->setBaud(921600); |
ayu13 | 0:83b2c6a67cce | 70 | nh.initNode(); |
ayu13 | 0:83b2c6a67cce | 71 | nh.subscribe(sub); |
ayu13 | 0:83b2c6a67cce | 72 | |
ayu13 | 0:83b2c6a67cce | 73 | timer.reset(); |
ayu13 | 0:83b2c6a67cce | 74 | timer.start(); |
ayu13 | 0:83b2c6a67cce | 75 | |
ayu13 | 0:83b2c6a67cce | 76 | motor.setEquation(0.0523,0.0148,0.0459,-0.0341); //Excel"omega_controll"の図より |
ayu13 | 0:83b2c6a67cce | 77 | |
ayu13 | 0:83b2c6a67cce | 78 | // target_rad = -(M_PI/6); //目標角度をπ/6(rad)に(単位に注意) ////////////////////////// |
ayu13 | 0:83b2c6a67cce | 79 | // target_deg = radToDeg(target_rad); |
ayu13 | 0:83b2c6a67cce | 80 | |
ayu13 | 0:83b2c6a67cce | 81 | while (1) { |
ayu13 | 0:83b2c6a67cce | 82 | //// ROS //// |
ayu13 | 0:83b2c6a67cce | 83 | nh.spinOnce(); |
ayu13 | 1:c0a3e4589a8f | 84 | target_rad = arm_rad; |
ayu13 | 1:c0a3e4589a8f | 85 | if(arm_num==1&&arm_rad==-(M_PI/6)) { //LEDは動作確認用 |
ayu13 | 0:83b2c6a67cce | 86 | myled2 = !myled2; |
ayu13 | 0:83b2c6a67cce | 87 | } |
ayu13 | 1:c0a3e4589a8f | 88 | |
ayu13 | 0:83b2c6a67cce | 89 | ////タッチセンサ//// |
ayu13 | 0:83b2c6a67cce | 90 | if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む |
ayu13 | 0:83b2c6a67cce | 91 | wait_us(50); |
ayu13 | 0:83b2c6a67cce | 92 | if(ts_c1 == 0) { |
ayu13 | 1:c0a3e4589a8f | 93 | servo_c.set_degree(0,servoDeg(10000)); //servo_c掴む |
ayu13 | 1:c0a3e4589a8f | 94 | hand = 1; |
ayu13 | 0:83b2c6a67cce | 95 | } |
ayu13 | 0:83b2c6a67cce | 96 | } |
ayu13 | 0:83b2c6a67cce | 97 | |
ayu13 | 0:83b2c6a67cce | 98 | ////アブソリュートエンコーダ//// |
ayu13 | 0:83b2c6a67cce | 99 | if(timer.read()>DELTA_T) { |
ayu13 | 0:83b2c6a67cce | 100 | if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す |
ayu13 | 1:c0a3e4589a8f | 101 | myled1=!myled1; //LED2 点灯 |
ayu13 | 0:83b2c6a67cce | 102 | motor.AcOmega(target_rad); //速度式角度制御 |
ayu13 | 0:83b2c6a67cce | 103 | } else { //目標角度に達したら停止 |
ayu13 | 0:83b2c6a67cce | 104 | myled1=0; //LED2 消灯 |
ayu13 | 0:83b2c6a67cce | 105 | motor.stop(); |
ayu13 | 1:c0a3e4589a8f | 106 | servo_c.set_degree(0,servoDeg(7500)); //servo_c離す |
ayu13 | 0:83b2c6a67cce | 107 | } |
ayu13 | 0:83b2c6a67cce | 108 | amt.rewriteCount(); |
ayu13 | 0:83b2c6a67cce | 109 | amt.calOmega(); |
ayu13 | 0:83b2c6a67cce | 110 | amt.getOmega(); |
ayu13 | 0:83b2c6a67cce | 111 | now_deg = amt.getDeg(); //度数法 |
ayu13 | 0:83b2c6a67cce | 112 | now_count=amt.getCount(); |
ayu13 | 0:83b2c6a67cce | 113 | motor.AcOmega(target_rad); |
ayu13 | 0:83b2c6a67cce | 114 | |
ayu13 | 0:83b2c6a67cce | 115 | // pc.printf("now_deg = %f,%f\r\n", now_deg,now_count); //現在角度取得 (確認以外ではコメントアウトしないとモーター回らない)//////////////////// |
ayu13 | 0:83b2c6a67cce | 116 | timer.reset(); |
ayu13 | 0:83b2c6a67cce | 117 | } |
ayu13 | 0:83b2c6a67cce | 118 | wait_us(10); |
ayu13 | 0:83b2c6a67cce | 119 | } |
ayu13 | 0:83b2c6a67cce | 120 | } |
ayu13 | 0:83b2c6a67cce | 121 | |
ayu13 | 0:83b2c6a67cce | 122 | ///////////////// 関数宣言部分 ////////////////////// |
ayu13 | 0:83b2c6a67cce | 123 | double servoDeg(double servo_value) |
ayu13 | 0:83b2c6a67cce | 124 | { |
ayu13 | 0:83b2c6a67cce | 125 | return (servo_value - 3500)*270/(11500 - 3500); |
ayu13 | 0:83b2c6a67cce | 126 | } |
ayu13 | 0:83b2c6a67cce | 127 | double radToDeg(double x) |
ayu13 | 0:83b2c6a67cce | 128 | { |
ayu13 | 0:83b2c6a67cce | 129 | return x*180/M_PI; |
ayu13 | 0:83b2c6a67cce | 130 | } |