kohei matsumoto / Mbed 2 deprecated NHK_CenterArm1

DRのPS3での操作用(アームの開閉をコントローラで制御可能)

Dependencies:   mbed CalPID motorout ros_lib_melodic MotorController_AbsEC

main.cpp@0:83b2c6a67cce, 2021-07-08 (annotated)

Committer:
ayu13
Date:
Thu Jul 08 03:46:29 2021 +0000
Revision:
0:83b2c6a67cce
Child:
1:c0a3e4589a8f
ROS_before

Who changed what in which revision?

UserRevisionLine numberNew 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 0:83b2c6a67cce 16 float arm_num=0,arm_deg=0;
ayu13 0:83b2c6a67cce 17 void cArmCallback(const geometry_msgs::Point &arm_state)
ayu13 0:83b2c6a67cce 18 {
ayu13 0:83b2c6a67cce 19 arm_num = arm_state.x;
ayu13 0:83b2c6a67cce 20 arm_deg = 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 0:83b2c6a67cce 29 #define DELTA_T 0.01 //制御周期
ayu13 0:83b2c6a67cce 30 #define DUTY_MAX 0.07 //duty比の最大値
ayu13 0:83b2c6a67cce 31 #define OMEGA_MAX 0.6 //速度制御を利用した角度制御での角速度の最大値
ayu13 0:83b2c6a67cce 32 #define NUM_DATA 270
ayu13 0:83b2c6a67cce 33
ayu13 0:83b2c6a67cce 34 Serial pc(USBTX, USBRX);
ayu13 0:83b2c6a67cce 35 ////角度制御(アブソリュートエンコーダ)/////
ayu13 0:83b2c6a67cce 36 DigitalOut myled1(LED1);
ayu13 0:83b2c6a67cce 37 Timer timer;
ayu13 0:83b2c6a67cce 38 CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX); //速度制御のPD計算
ayu13 0:83b2c6a67cce 39 CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX); //角度制御（duty式）のPD計算 不要？
ayu13 0:83b2c6a67cce 40 CalPID angle_omega_pid(5.5,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
ayu13 0:83b2c6a67cce 41 Amt21 amt(p9,p10,p8); //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode)
ayu13 0:83b2c6a67cce 42 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 43 double target_speed=0;
ayu13 0:83b2c6a67cce 44 double target_rad=0;
ayu13 0:83b2c6a67cce 45 double target_deg = 0;
ayu13 0:83b2c6a67cce 46 double radToDeg(double x);
ayu13 0:83b2c6a67cce 47
ayu13 0:83b2c6a67cce 48 ////先端アーム(サーボ)/////
ayu13 0:83b2c6a67cce 49 DigitalIn ts_c1(p6, PullUp);
ayu13 0:83b2c6a67cce 50 DigitalIn ts_c2(p7, PullUp);
ayu13 0:83b2c6a67cce 51 KondoServo servo_c(p13, p14);
ayu13 0:83b2c6a67cce 52 double servoDeg(double servo_value);
ayu13 0:83b2c6a67cce 53 double calServoDeg(double servo_value); //360°の度数法にする
ayu13 0:83b2c6a67cce 54 int id_c1 = 0;
ayu13 0:83b2c6a67cce 55 float deg_c_grab = 9000;
ayu13 0:83b2c6a67cce 56 float deg_c_open = 3500;
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 0:83b2c6a67cce 69 nh.getHardware()->setBaud(115200);
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 0:83b2c6a67cce 84 if(arm_num==1&&arm_deg==60) { //LEDは動作確認用
ayu13 0:83b2c6a67cce 85 myled2 = !myled2;
ayu13 0:83b2c6a67cce 86 }
ayu13 0:83b2c6a67cce 87 if(arm_num == 1) {
ayu13 0:83b2c6a67cce 88 myled3 = 1;
ayu13 0:83b2c6a67cce 89 }
ayu13 0:83b2c6a67cce 90 if(arm_deg == 60) {
ayu13 0:83b2c6a67cce 91 myled4 = 1;
ayu13 0:83b2c6a67cce 92 }
ayu13 0:83b2c6a67cce 93
ayu13 0:83b2c6a67cce 94 ////タッチセンサ////
ayu13 0:83b2c6a67cce 95 if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む
ayu13 0:83b2c6a67cce 96 wait_us(50);
ayu13 0:83b2c6a67cce 97 if(ts_c1 == 0) {
ayu13 0:83b2c6a67cce 98 servo_c.set_degree(0,servoDeg(4500)); //servo_c掴む
ayu13 0:83b2c6a67cce 99 }
ayu13 0:83b2c6a67cce 100 }
ayu13 0:83b2c6a67cce 101
ayu13 0:83b2c6a67cce 102 ////アブソリュートエンコーダ////
ayu13 0:83b2c6a67cce 103 if(timer.read()>DELTA_T) {
ayu13 0:83b2c6a67cce 104 if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す
ayu13 0:83b2c6a67cce 105 myled1=1; //LED2 点灯
ayu13 0:83b2c6a67cce 106 motor.AcOmega(target_rad); //速度式角度制御
ayu13 0:83b2c6a67cce 107 } else { //目標角度に達したら停止
ayu13 0:83b2c6a67cce 108 myled1=0; //LED2 消灯
ayu13 0:83b2c6a67cce 109 motor.stop();
ayu13 0:83b2c6a67cce 110 servo_c.set_degree(0,servoDeg(1000)); //servo_c離す
ayu13 0:83b2c6a67cce 111 }
ayu13 0:83b2c6a67cce 112 amt.rewriteCount();
ayu13 0:83b2c6a67cce 113 amt.calOmega();
ayu13 0:83b2c6a67cce 114 amt.getOmega();
ayu13 0:83b2c6a67cce 115 now_deg = amt.getDeg(); //度数法
ayu13 0:83b2c6a67cce 116 now_count=amt.getCount();
ayu13 0:83b2c6a67cce 117 motor.AcOmega(target_rad);
ayu13 0:83b2c6a67cce 118
ayu13 0:83b2c6a67cce 119 // pc.printf("now_deg = %f,%f\r\n", now_deg,now_count); //現在角度取得 (確認以外ではコメントアウトしないとモーター回らない)////////////////////
ayu13 0:83b2c6a67cce 120 timer.reset();
ayu13 0:83b2c6a67cce 121 }
ayu13 0:83b2c6a67cce 122 wait_us(10);
ayu13 0:83b2c6a67cce 123 }
ayu13 0:83b2c6a67cce 124 }
ayu13 0:83b2c6a67cce 125
ayu13 0:83b2c6a67cce 126 ///////////////// 関数宣言部分 //////////////////////
ayu13 0:83b2c6a67cce 127 double servoDeg(double servo_value)
ayu13 0:83b2c6a67cce 128 {
ayu13 0:83b2c6a67cce 129 return (servo_value - 3500)*270/(11500 - 3500);
ayu13 0:83b2c6a67cce 130 }
ayu13 0:83b2c6a67cce 131 double radToDeg(double x)
ayu13 0:83b2c6a67cce 132 {
ayu13 0:83b2c6a67cce 133 return x*180/M_PI;
ayu13 0:83b2c6a67cce 134 }