kohei matsumoto / Mbed 2 deprecated NHK_CenterArm

DRのPS3での操作用

Dependencies:   mbed CalPID motorout ros_lib_melodic MotorController_AbsEC

main.cpp@7:038ea29c5a97, 2021-07-20 (annotated)

Committer:
ayu13
Date:
Tue Jul 20 05:02:52 2021 +0000
Revision:
7:038ea29c5a97
Parent:
6:e6b303ea6439
Child:
8:9656790eea17
ROS_OK_middle

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
ayu13 5:4fba3e91741f 21 int state = 0;
ayu13 5:4fba3e91741f 22
ayu13 0:83b2c6a67cce 23 ////角度制御(アブソリュートエンコーダ)/////
ayu13 0:83b2c6a67cce 24 DigitalOut myled1(LED1);
ayu13 0:83b2c6a67cce 25 Timer timer;
ayu13 0:83b2c6a67cce 26 CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX); //速度制御のPD計算
ayu13 3:78b608ba0286 27 CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX); //角度制御（duty式）のPD計算
ayu13 6:e6b303ea6439 28 CalPID angle_omega_pid(5.0,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
ayu13 0:83b2c6a67cce 29 Amt21 amt(p9,p10,p8); //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode)
ayu13 0:83b2c6a67cce 30 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 31 double radToDeg(double x);
ayu13 5:4fba3e91741f 32 double degToRad(double x);
ayu13 0:83b2c6a67cce 33 double target_speed=0;
ayu13 0:83b2c6a67cce 34 double target_rad=0;
ayu13 0:83b2c6a67cce 35 double target_deg = 0;
ayu13 5:4fba3e91741f 36 double right_deg = -90;
ayu13 5:4fba3e91741f 37 double left_deg = 90;
ayu13 7:038ea29c5a97 38 double turn_deg = 0;
ayu13 7:038ea29c5a97 39 double turn_rad = 0;
ayu13 0:83b2c6a67cce 40
ayu13 0:83b2c6a67cce 41 ////先端アーム(サーボ)/////
ayu13 0:83b2c6a67cce 42 DigitalIn ts_c1(p6, PullUp);
ayu13 0:83b2c6a67cce 43 DigitalIn ts_c2(p7, PullUp);
ayu13 0:83b2c6a67cce 44 KondoServo servo_c(p13, p14);
ayu13 0:83b2c6a67cce 45 double servoDeg(double servo_value);
ayu13 5:4fba3e91741f 46 void touchGrab();
ayu13 0:83b2c6a67cce 47 int id_c1 = 0;
ayu13 6:e6b303ea6439 48 float deg_c_grab = 10600;
ayu13 6:e6b303ea6439 49 float deg_c_open = 8300;
ayu13 4:3abefa13c2e3 50 int hand = 0; //タッチセンサがONになった状態:1，OFF:0
ayu13 0:83b2c6a67cce 51
ayu13 0:83b2c6a67cce 52 ////データ記録まわり/////
ayu13 0:83b2c6a67cce 53 int angle_count=0;
ayu13 0:83b2c6a67cce 54 int omega_count=0;
ayu13 4:3abefa13c2e3 55 double now_deg=0;
ayu13 0:83b2c6a67cce 56 double now_count=0;
ayu13 0:83b2c6a67cce 57
ayu13 2:b728a6564520 58 ///////////////// ROS /////////////////
ayu13 2:b728a6564520 59 DigitalOut myled2(LED2);
ayu13 2:b728a6564520 60 DigitalOut myled3(LED3);
ayu13 2:b728a6564520 61 DigitalOut myled4(LED4);
ayu13 7:038ea29c5a97 62 float arm_turn=0,arm_deg=0,arm_rad=0;
ayu13 2:b728a6564520 63 void cArmCallback(const geometry_msgs::Point &arm_state)
ayu13 2:b728a6564520 64 {
ayu13 7:038ea29c5a97 65 arm_turn = arm_state.x; //右回転＝1、左回転＝2
ayu13 2:b728a6564520 66 arm_deg = arm_state.y; //目標角度
ayu13 2:b728a6564520 67 }
ayu13 2:b728a6564520 68 ros::NodeHandle nh;
ayu13 2:b728a6564520 69 ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
ayu13 2:b728a6564520 70
ayu13 6:e6b303ea6439 71 DigitalIn toggle1(p16, PullUp); //right
ayu13 6:e6b303ea6439 72 DigitalIn toggle2(p20, PullUp); //left
ayu13 6:e6b303ea6439 73
ayu13 0:83b2c6a67cce 74 int main()
ayu13 0:83b2c6a67cce 75 {
ayu13 5:4fba3e91741f 76 myled1 = 0; //目標角度以上離れている:点灯
ayu13 5:4fba3e91741f 77 myled2 = 0; //通信で欲しい値が送られているとき:点灯
ayu13 5:4fba3e91741f 78 myled3 = 0; //
ayu13 5:4fba3e91741f 79 myled4 = 0; //サーボを掴む→点灯．離す→消灯
ayu13 5:4fba3e91741f 80
ayu13 0:83b2c6a67cce 81 ///////// ROS /////////
ayu13 1:c0a3e4589a8f 82 nh.getHardware()->setBaud(921600);
ayu13 0:83b2c6a67cce 83 nh.initNode();
ayu13 0:83b2c6a67cce 84 nh.subscribe(sub);
ayu13 0:83b2c6a67cce 85
ayu13 0:83b2c6a67cce 86 timer.reset();
ayu13 0:83b2c6a67cce 87 timer.start();
ayu13 0:83b2c6a67cce 88
ayu13 0:83b2c6a67cce 89 motor.setEquation(0.0523,0.0148,0.0459,-0.0341); //Excel"omega_controll"の図より
ayu13 0:83b2c6a67cce 90
ayu13 3:78b608ba0286 91 // target_rad = -(M_PI/6); //目標角度をπ/6（rad）に（単位に注意） //////////////////////////
ayu13 3:78b608ba0286 92 // target_deg = radToDeg(target_rad);
ayu13 0:83b2c6a67cce 93
ayu13 0:83b2c6a67cce 94 while (1) {
ayu13 0:83b2c6a67cce 95 //// ROS ////
ayu13 0:83b2c6a67cce 96 nh.spinOnce();
ayu13 4:3abefa13c2e3 97
ayu13 5:4fba3e91741f 98 if(timer.read()>DELTA_T) {
ayu13 7:038ea29c5a97 99 if(arm_turn==2&&arm_deg==-10) { //動作確認用
ayu13 5:4fba3e91741f 100 myled2 = 1;
ayu13 0:83b2c6a67cce 101 }
ayu13 5:4fba3e91741f 102 arm_rad = degToRad(arm_deg);
ayu13 7:038ea29c5a97 103 target_deg = arm_deg;
ayu13 7:038ea29c5a97 104 target_rad = arm_rad;
ayu13 0:83b2c6a67cce 105
ayu13 5:4fba3e91741f 106 if(state == 0) {
ayu13 6:e6b303ea6439 107 ///トグルスイッチ1////1
ayu13 6:e6b303ea6439 108 if(toggle1 == 0) { //トグルスイッチ1(mbed側)ONで初start
ayu13 6:e6b303ea6439 109 wait_us(50);
ayu13 6:e6b303ea6439 110 if(toggle1 == 0) {
ayu13 6:e6b303ea6439 111 state ++;
ayu13 6:e6b303ea6439 112 }
ayu13 6:e6b303ea6439 113 }
ayu13 6:e6b303ea6439 114 }
ayu13 6:e6b303ea6439 115 if(state == 1) {
ayu13 7:038ea29c5a97 116 if(arm_turn == 1) {
ayu13 7:038ea29c5a97 117 turn_deg = 90;
ayu13 7:038ea29c5a97 118 } else if(arm_turn == 2) {
ayu13 7:038ea29c5a97 119 turn_deg = -90;
ayu13 7:038ea29c5a97 120 }
ayu13 7:038ea29c5a97 121 turn_rad = degToRad(turn_deg);
ayu13 6:e6b303ea6439 122 servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す
ayu13 5:4fba3e91741f 123 state++;
ayu13 6:e6b303ea6439 124 } else if (state == 2) {
ayu13 5:4fba3e91741f 125 myled3 = 1;
ayu13 7:038ea29c5a97 126 if(fabs(now_deg-turn_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す
ayu13 5:4fba3e91741f 127 myled1 = 1; //LED1 点灯
ayu13 7:038ea29c5a97 128 motor.AcOmega(turn_rad); //速度式角度制御
ayu13 5:4fba3e91741f 129 } else { //目標角度に達したら停止
ayu13 5:4fba3e91741f 130 myled1=0; //LED1 消灯
ayu13 5:4fba3e91741f 131 motor.stop();
ayu13 5:4fba3e91741f 132 }
ayu13 5:4fba3e91741f 133 touchGrab(); //タッチセンサ
ayu13 6:e6b303ea6439 134 } else if (state == 3) {
ayu13 5:4fba3e91741f 135 myled3 = 0;
ayu13 5:4fba3e91741f 136 target_rad = arm_rad;
ayu13 5:4fba3e91741f 137 target_deg = arm_deg;
ayu13 5:4fba3e91741f 138 state++;
ayu13 6:e6b303ea6439 139 } else if (state == 4) {
ayu13 5:4fba3e91741f 140 if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す
ayu13 5:4fba3e91741f 141 myled1 = 1; //LED1 点灯
ayu13 3:78b608ba0286 142 motor.AcOmega(target_rad); //速度式角度制御
ayu13 5:4fba3e91741f 143 } else { //目標角度に達したら停止
ayu13 5:4fba3e91741f 144 myled1=0; //LED1 消灯
ayu13 5:4fba3e91741f 145 motor.stop();
ayu13 5:4fba3e91741f 146 //wait(100); //table停止用時間
ayu13 5:4fba3e91741f 147 servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す
ayu13 5:4fba3e91741f 148 myled4 = 0;
ayu13 6:e6b303ea6439 149 // state = 0;
ayu13 3:78b608ba0286 150 }
ayu13 0:83b2c6a67cce 151 }
ayu13 5:4fba3e91741f 152
ayu13 5:4fba3e91741f 153 ////タッチセンサ////
ayu13 5:4fba3e91741f 154 // touchGrab();
ayu13 5:4fba3e91741f 155
ayu13 5:4fba3e91741f 156 ////アブソリュートエンコーダ////
ayu13 5:4fba3e91741f 157
ayu13 5:4fba3e91741f 158 // if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す
ayu13 5:4fba3e91741f 159 // myled1 = 1; //LED1 点灯
ayu13 5:4fba3e91741f 160 //// if(hand == 1) { //タッチセンサが反応していたとき(ハンドルを掴んでいたとき)
ayu13 5:4fba3e91741f 161 // motor.AcOmega(target_rad); //速度式角度制御
ayu13 5:4fba3e91741f 162 //// } else if(hand == 0) {
ayu13 5:4fba3e91741f 163 //// myled4=0; //LED4 消灯
ayu13 5:4fba3e91741f 164 //// }
ayu13 5:4fba3e91741f 165 // } else if(fabs(now_deg-target_deg) < 0.5 && state == 3) { //目標角度に達したら停止
ayu13 5:4fba3e91741f 166 // myled1=0; //LED1 消灯
ayu13 5:4fba3e91741f 167 // motor.stop();
ayu13 5:4fba3e91741f 168 // hand = 0;
ayu13 5:4fba3e91741f 169 // //wait(100); //table停止用時間
ayu13 5:4fba3e91741f 170 // servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す
ayu13 5:4fba3e91741f 171 // }
ayu13 0:83b2c6a67cce 172 amt.rewriteCount();
ayu13 0:83b2c6a67cce 173 amt.calOmega();
ayu13 0:83b2c6a67cce 174 amt.getOmega();
ayu13 4:3abefa13c2e3 175 now_deg = amt.getDeg();
ayu13 0:83b2c6a67cce 176 now_count=amt.getCount();
ayu13 4:3abefa13c2e3 177 // motor.AcOmega(target_rad);
ayu13 0:83b2c6a67cce 178
ayu13 0:83b2c6a67cce 179 timer.reset();
ayu13 0:83b2c6a67cce 180 }
ayu13 0:83b2c6a67cce 181 wait_us(10);
ayu13 0:83b2c6a67cce 182 }
ayu13 0:83b2c6a67cce 183 }
ayu13 0:83b2c6a67cce 184
ayu13 0:83b2c6a67cce 185 ///////////////// 関数宣言部分 //////////////////////
ayu13 0:83b2c6a67cce 186 double servoDeg(double servo_value)
ayu13 0:83b2c6a67cce 187 {
ayu13 0:83b2c6a67cce 188 return (servo_value - 3500)*270/(11500 - 3500);
ayu13 0:83b2c6a67cce 189 }
ayu13 0:83b2c6a67cce 190 double radToDeg(double x)
ayu13 0:83b2c6a67cce 191 {
ayu13 0:83b2c6a67cce 192 return x*180/M_PI;
ayu13 2:b728a6564520 193 }
ayu13 2:b728a6564520 194 double degToRad(double x)
ayu13 2:b728a6564520 195 {
ayu13 2:b728a6564520 196 return x*M_PI/180;
ayu13 5:4fba3e91741f 197 }
ayu13 5:4fba3e91741f 198 void touchGrab()
ayu13 5:4fba3e91741f 199 {
ayu13 5:4fba3e91741f 200 if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む
ayu13 5:4fba3e91741f 201 wait_us(50);
ayu13 5:4fba3e91741f 202 if(ts_c1 == 0) {
ayu13 5:4fba3e91741f 203 servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む
ayu13 5:4fba3e91741f 204 state++;
ayu13 5:4fba3e91741f 205 myled4=1; //LED4 点灯
ayu13 5:4fba3e91741f 206 }
ayu13 5:4fba3e91741f 207 }
ayu13 0:83b2c6a67cce 208 }