kohei matsumoto / Mbed 2 deprecated NHK_CenterArm1

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?

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 #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 }