![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用(アームの開閉をコントローラで制御可能)
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
main.cpp@12:c2bb0a9587b5, 2021-07-30 (annotated)
- Committer:
- koheim
- Date:
- Fri Jul 30 11:36:46 2021 +0000
- Revision:
- 12:c2bb0a9587b5
- Parent:
- 11:df2acf22123e
a
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 | #include <ros.h> |
ayu13 | 0:83b2c6a67cce | 9 | #include <std_msgs/String.h> |
ayu13 | 0:83b2c6a67cce | 10 | #include <geometry_msgs/Point.h> |
ayu13 | 0:83b2c6a67cce | 11 | |
ayu13 | 0:83b2c6a67cce | 12 | ///////////////// 宣言部分 ////////////////////// |
ayu13 | 0:83b2c6a67cce | 13 | #ifndef M_PI |
ayu13 | 0:83b2c6a67cce | 14 | #define M_PI 3.14159265359f |
ayu13 | 0:83b2c6a67cce | 15 | #endif |
ayu13 | 1:c0a3e4589a8f | 16 | #define DELTA_T 0.01 //制御周期 |
ayu13 | 1:c0a3e4589a8f | 17 | #define DUTY_MAX 0.07 //duty比の最大値 |
ayu13 | 6:e6b303ea6439 | 18 | #define OMEGA_MAX 0.25 //速度制御を利用した角度制御での角速度の最大値 |
ayu13 | 0:83b2c6a67cce | 19 | #define NUM_DATA 270 |
ayu13 | 0:83b2c6a67cce | 20 | |
koheim | 11:df2acf22123e | 21 | int state = 1; |
ayu13 | 5:4fba3e91741f | 22 | |
ayu13 | 0:83b2c6a67cce | 23 | ////角度制御(アブソリュートエンコーダ)///// |
ayu13 | 0:83b2c6a67cce | 24 | Timer timer; |
ayu13 | 0:83b2c6a67cce | 25 | CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX); //速度制御のPD計算 |
ayu13 | 3:78b608ba0286 | 26 | CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 |
ayu13 | 6:e6b303ea6439 | 27 | CalPID angle_omega_pid(5.0,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 |
ayu13 | 0:83b2c6a67cce | 28 | Amt21 amt(p9,p10,p8); //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode) |
ayu13 | 0:83b2c6a67cce | 29 | 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 | 5:4fba3e91741f | 30 | double degToRad(double x); |
ayu13 | 0:83b2c6a67cce | 31 | double target_rad=0; |
ayu13 | 0:83b2c6a67cce | 32 | double target_deg = 0; |
ayu13 | 7:038ea29c5a97 | 33 | double turn_deg = 0; |
ayu13 | 7:038ea29c5a97 | 34 | double turn_rad = 0; |
ayu13 | 9:95f6b9f54395 | 35 | DigitalIn toggle1(p16, PullUp); //トグルright(マイコン側) |
ayu13 | 9:95f6b9f54395 | 36 | DigitalIn toggle2(p20, PullUp); //トグルleft(外側) |
ayu13 | 0:83b2c6a67cce | 37 | |
ayu13 | 0:83b2c6a67cce | 38 | ////先端アーム(サーボ)///// |
ayu13 | 0:83b2c6a67cce | 39 | DigitalIn ts_c1(p6, PullUp); |
ayu13 | 0:83b2c6a67cce | 40 | DigitalIn ts_c2(p7, PullUp); |
ayu13 | 0:83b2c6a67cce | 41 | KondoServo servo_c(p13, p14); |
ayu13 | 0:83b2c6a67cce | 42 | double servoDeg(double servo_value); |
ayu13 | 10:c1ca3db7177c | 43 | void touchGrab(); //タッチセンサ反応したらサーボ掴む |
koheim | 11:df2acf22123e | 44 | float deg_c_grab = 11000; |
ayu13 | 6:e6b303ea6439 | 45 | float deg_c_open = 8300; |
ayu13 | 0:83b2c6a67cce | 46 | |
ayu13 | 9:95f6b9f54395 | 47 | ////データ記録関連///// |
ayu13 | 4:3abefa13c2e3 | 48 | double now_deg=0; |
ayu13 | 0:83b2c6a67cce | 49 | double now_count=0; |
ayu13 | 0:83b2c6a67cce | 50 | |
koheim | 12:c2bb0a9587b5 | 51 | int catch_mode = 0; |
koheim | 12:c2bb0a9587b5 | 52 | |
ayu13 | 2:b728a6564520 | 53 | ///////////////// ROS ///////////////// |
ayu13 | 9:95f6b9f54395 | 54 | DigitalOut myled1(LED1); |
ayu13 | 2:b728a6564520 | 55 | DigitalOut myled2(LED2); |
ayu13 | 2:b728a6564520 | 56 | DigitalOut myled3(LED3); |
ayu13 | 2:b728a6564520 | 57 | DigitalOut myled4(LED4); |
ayu13 | 7:038ea29c5a97 | 58 | float arm_turn=0,arm_deg=0,arm_rad=0; |
ayu13 | 2:b728a6564520 | 59 | void cArmCallback(const geometry_msgs::Point &arm_state) |
ayu13 | 2:b728a6564520 | 60 | { |
ayu13 | 10:c1ca3db7177c | 61 | arm_turn = arm_state.x; //左回転=1、右回転=2 |
ayu13 | 10:c1ca3db7177c | 62 | arm_deg = arm_state.y; //目標角度 |
koheim | 12:c2bb0a9587b5 | 63 | catch_mode = arm_state.z; |
ayu13 | 2:b728a6564520 | 64 | } |
koheim | 12:c2bb0a9587b5 | 65 | |
ayu13 | 2:b728a6564520 | 66 | ros::NodeHandle nh; |
ayu13 | 2:b728a6564520 | 67 | ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); |
ayu13 | 2:b728a6564520 | 68 | |
ayu13 | 0:83b2c6a67cce | 69 | int main() |
ayu13 | 0:83b2c6a67cce | 70 | { |
koheim | 11:df2acf22123e | 71 | int mode = 0; |
ayu13 | 5:4fba3e91741f | 72 | myled1 = 0; //目標角度以上離れている:点灯 |
ayu13 | 5:4fba3e91741f | 73 | myled2 = 0; //通信で欲しい値が送られているとき:点灯 |
ayu13 | 8:9656790eea17 | 74 | myled3 = 0; //state2に入ったとき←基本不要 |
ayu13 | 5:4fba3e91741f | 75 | myled4 = 0; //サーボを掴む→点灯.離す→消灯 |
ayu13 | 5:4fba3e91741f | 76 | |
ayu13 | 0:83b2c6a67cce | 77 | ///////// ROS ///////// |
koheim | 11:df2acf22123e | 78 | nh.getHardware()->setBaud(115200); |
ayu13 | 0:83b2c6a67cce | 79 | nh.initNode(); |
ayu13 | 0:83b2c6a67cce | 80 | nh.subscribe(sub); |
ayu13 | 0:83b2c6a67cce | 81 | |
ayu13 | 0:83b2c6a67cce | 82 | timer.reset(); |
ayu13 | 0:83b2c6a67cce | 83 | timer.start(); |
ayu13 | 0:83b2c6a67cce | 84 | |
ayu13 | 0:83b2c6a67cce | 85 | motor.setEquation(0.0523,0.0148,0.0459,-0.0341); //Excel"omega_controll"の図より |
koheim | 11:df2acf22123e | 86 | //////////////////////////////////////// |
koheim | 11:df2acf22123e | 87 | /*arm_turn = 2; //左回転=1、右回転=2 |
koheim | 11:df2acf22123e | 88 | arm_deg = -90; //目標角度*/ |
koheim | 11:df2acf22123e | 89 | /////////////////////////////////////// |
ayu13 | 0:83b2c6a67cce | 90 | |
ayu13 | 0:83b2c6a67cce | 91 | while (1) { |
ayu13 | 8:9656790eea17 | 92 | nh.spinOnce(); //ROS |
koheim | 12:c2bb0a9587b5 | 93 | |
koheim | 12:c2bb0a9587b5 | 94 | if(catch_mode == 1){ |
koheim | 12:c2bb0a9587b5 | 95 | servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す |
koheim | 12:c2bb0a9587b5 | 96 | myled1 = 1; |
koheim | 12:c2bb0a9587b5 | 97 | } |
koheim | 12:c2bb0a9587b5 | 98 | else if(catch_mode == 2){ |
koheim | 12:c2bb0a9587b5 | 99 | servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む |
koheim | 12:c2bb0a9587b5 | 100 | myled2 = 1; |
koheim | 12:c2bb0a9587b5 | 101 | } |
ayu13 | 4:3abefa13c2e3 | 102 | |
ayu13 | 5:4fba3e91741f | 103 | if(timer.read()>DELTA_T) { |
ayu13 | 8:9656790eea17 | 104 | if(arm_turn==2&&arm_deg==-10) { //動作確認(通信)用 |
ayu13 | 5:4fba3e91741f | 105 | myled2 = 1; |
ayu13 | 0:83b2c6a67cce | 106 | } |
ayu13 | 5:4fba3e91741f | 107 | arm_rad = degToRad(arm_deg); |
ayu13 | 7:038ea29c5a97 | 108 | target_deg = arm_deg; |
ayu13 | 7:038ea29c5a97 | 109 | target_rad = arm_rad; |
ayu13 | 0:83b2c6a67cce | 110 | |
ayu13 | 10:c1ca3db7177c | 111 | if(state == 0) { //トグルスイッチONでstate=1へ. |
ayu13 | 8:9656790eea17 | 112 | ///トグルスイッチ1////マイコン方向 |
ayu13 | 10:c1ca3db7177c | 113 | if(toggle1 == 0) { //トグルスイッチ1(mbed側)ONで初start |
ayu13 | 6:e6b303ea6439 | 114 | wait_us(50); |
ayu13 | 6:e6b303ea6439 | 115 | if(toggle1 == 0) { |
ayu13 | 10:c1ca3db7177c | 116 | state++; |
ayu13 | 6:e6b303ea6439 | 117 | } |
ayu13 | 6:e6b303ea6439 | 118 | } |
ayu13 | 6:e6b303ea6439 | 119 | } |
ayu13 | 10:c1ca3db7177c | 120 | if(state == 1) { //回転方向の変数"arm_turn"に1か2が代入されたらstate=2へ. |
ayu13 | 7:038ea29c5a97 | 121 | if(arm_turn == 1) { |
ayu13 | 7:038ea29c5a97 | 122 | turn_deg = 90; |
ayu13 | 10:c1ca3db7177c | 123 | state++; |
ayu13 | 7:038ea29c5a97 | 124 | } else if(arm_turn == 2) { |
ayu13 | 7:038ea29c5a97 | 125 | turn_deg = -90; |
ayu13 | 10:c1ca3db7177c | 126 | state++; |
ayu13 | 7:038ea29c5a97 | 127 | } |
ayu13 | 7:038ea29c5a97 | 128 | turn_rad = degToRad(turn_deg); |
ayu13 | 10:c1ca3db7177c | 129 | } else if(state == 2) { //サーボを離してそのまますぐstate=3へ. |
ayu13 | 10:c1ca3db7177c | 130 | servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す |
ayu13 | 5:4fba3e91741f | 131 | state++; |
ayu13 | 10:c1ca3db7177c | 132 | } else if (state == 3) {//タッチセンサがハンドルに反応したらサーボで掴んでstate=4へ. |
ayu13 | 5:4fba3e91741f | 133 | myled3 = 1; |
ayu13 | 10:c1ca3db7177c | 134 | if(fabs(now_deg-turn_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す |
ayu13 | 10:c1ca3db7177c | 135 | myled1 = 1; //LED1 点灯 |
ayu13 | 10:c1ca3db7177c | 136 | motor.AcOmega(turn_rad); //速度式角度制御 |
ayu13 | 5:4fba3e91741f | 137 | } else { //目標角度に達したら停止 |
ayu13 | 5:4fba3e91741f | 138 | myled1=0; //LED1 消灯 |
ayu13 | 5:4fba3e91741f | 139 | motor.stop(); |
ayu13 | 5:4fba3e91741f | 140 | } |
koheim | 12:c2bb0a9587b5 | 141 | state++; |
koheim | 12:c2bb0a9587b5 | 142 | //touchGrab(); |
ayu13 | 10:c1ca3db7177c | 143 | } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ. |
ayu13 | 5:4fba3e91741f | 144 | myled3 = 0; |
koheim | 11:df2acf22123e | 145 | if(mode == 0){ |
koheim | 11:df2acf22123e | 146 | target_rad = arm_rad; |
koheim | 11:df2acf22123e | 147 | target_deg = arm_deg; |
koheim | 11:df2acf22123e | 148 | } |
koheim | 11:df2acf22123e | 149 | else if(mode == 1){ |
koheim | 11:df2acf22123e | 150 | target_deg = 45; |
koheim | 11:df2acf22123e | 151 | target_rad = degToRad(target_deg); |
koheim | 11:df2acf22123e | 152 | } |
koheim | 11:df2acf22123e | 153 | else if(mode == 2){ |
koheim | 11:df2acf22123e | 154 | target_deg = -45; |
koheim | 11:df2acf22123e | 155 | target_rad = degToRad(target_deg); |
koheim | 11:df2acf22123e | 156 | } |
ayu13 | 5:4fba3e91741f | 157 | state++; |
koheim | 12:c2bb0a9587b5 | 158 | } else if (state == 5) {//目標角度に近づいたら停止する |
ayu13 | 10:c1ca3db7177c | 159 | if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す |
ayu13 | 10:c1ca3db7177c | 160 | myled1 = 1; //LED1 点灯 |
ayu13 | 10:c1ca3db7177c | 161 | motor.AcOmega(target_rad); //速度式角度制御 |
ayu13 | 5:4fba3e91741f | 162 | } else { //目標角度に達したら停止 |
ayu13 | 10:c1ca3db7177c | 163 | myled1 = 0; //LED1 消灯 |
ayu13 | 5:4fba3e91741f | 164 | motor.stop(); |
koheim | 11:df2acf22123e | 165 | //servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す |
ayu13 | 5:4fba3e91741f | 166 | myled4 = 0; |
koheim | 11:df2acf22123e | 167 | if(mode == 0){ |
koheim | 11:df2acf22123e | 168 | mode = 1; |
koheim | 11:df2acf22123e | 169 | } |
koheim | 11:df2acf22123e | 170 | else{ |
koheim | 11:df2acf22123e | 171 | mode = 3-mode; |
koheim | 11:df2acf22123e | 172 | } |
koheim | 11:df2acf22123e | 173 | state = 4; /////////////////////連続して動かす時はここのコメントアウト外す. |
ayu13 | 3:78b608ba0286 | 174 | } |
ayu13 | 0:83b2c6a67cce | 175 | } |
ayu13 | 5:4fba3e91741f | 176 | |
ayu13 | 0:83b2c6a67cce | 177 | amt.rewriteCount(); |
ayu13 | 0:83b2c6a67cce | 178 | amt.calOmega(); |
ayu13 | 4:3abefa13c2e3 | 179 | now_deg = amt.getDeg(); |
ayu13 | 0:83b2c6a67cce | 180 | now_count=amt.getCount(); |
ayu13 | 0:83b2c6a67cce | 181 | |
ayu13 | 0:83b2c6a67cce | 182 | timer.reset(); |
ayu13 | 0:83b2c6a67cce | 183 | } |
ayu13 | 0:83b2c6a67cce | 184 | wait_us(10); |
ayu13 | 0:83b2c6a67cce | 185 | } |
ayu13 | 0:83b2c6a67cce | 186 | } |
ayu13 | 0:83b2c6a67cce | 187 | |
ayu13 | 0:83b2c6a67cce | 188 | ///////////////// 関数宣言部分 ////////////////////// |
ayu13 | 0:83b2c6a67cce | 189 | double servoDeg(double servo_value) |
ayu13 | 0:83b2c6a67cce | 190 | { |
ayu13 | 0:83b2c6a67cce | 191 | return (servo_value - 3500)*270/(11500 - 3500); |
ayu13 | 0:83b2c6a67cce | 192 | } |
ayu13 | 2:b728a6564520 | 193 | double degToRad(double x) |
ayu13 | 2:b728a6564520 | 194 | { |
ayu13 | 2:b728a6564520 | 195 | return x*M_PI/180; |
ayu13 | 5:4fba3e91741f | 196 | } |
ayu13 | 5:4fba3e91741f | 197 | void touchGrab() |
ayu13 | 5:4fba3e91741f | 198 | { |
ayu13 | 5:4fba3e91741f | 199 | if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む |
ayu13 | 5:4fba3e91741f | 200 | wait_us(50); |
ayu13 | 5:4fba3e91741f | 201 | if(ts_c1 == 0) { |
ayu13 | 5:4fba3e91741f | 202 | servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む |
ayu13 | 5:4fba3e91741f | 203 | state++; |
ayu13 | 5:4fba3e91741f | 204 | myled4=1; //LED4 点灯 |
ayu13 | 5:4fba3e91741f | 205 | } |
ayu13 | 5:4fba3e91741f | 206 | } |
ayu13 | 0:83b2c6a67cce | 207 | } |