![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
DRのPS3での操作用(アームの開閉をコントローラで制御可能)
Dependencies: mbed CalPID motorout ros_lib_melodic MotorController_AbsEC
main.cpp
- Committer:
- koheim
- Date:
- 2021-07-25
- Revision:
- 11:df2acf22123e
- Parent:
- 10:c1ca3db7177c
- Child:
- 12:c2bb0a9587b5
File content as of revision 11:df2acf22123e:
////c = center arm #include "mbed.h" #include "AMT21.h" #include "CalPID.h" #include "KondoServo.h" #include "MotorController.h" #include <stdlib.h> #include <ros.h> #include <std_msgs/String.h> #include <geometry_msgs/Point.h> ///////////////// 宣言部分 ////////////////////// #ifndef M_PI #define M_PI 3.14159265359f #endif #define DELTA_T 0.01 //制御周期 #define DUTY_MAX 0.07 //duty比の最大値 #define OMEGA_MAX 0.25 //速度制御を利用した角度制御での角速度の最大値 #define NUM_DATA 270 int state = 1; ////角度制御(アブソリュートエンコーダ)///// Timer timer; CalPID speed_pid(0.08,0,0.0008,DELTA_T,DUTY_MAX); //速度制御のPD計算 CalPID angle_duty_pid(0,0,0,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 CalPID angle_omega_pid(5.0,0,0.0032,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 Amt21 amt(p9,p10,p8); //Amt21 (PinName tx,PinName rx,PinName mode):serial_(tx,rx),rs485_mode(mode) 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) double degToRad(double x); double target_rad=0; double target_deg = 0; double turn_deg = 0; double turn_rad = 0; DigitalIn toggle1(p16, PullUp); //トグルright(マイコン側) DigitalIn toggle2(p20, PullUp); //トグルleft(外側) ////先端アーム(サーボ)///// DigitalIn ts_c1(p6, PullUp); DigitalIn ts_c2(p7, PullUp); KondoServo servo_c(p13, p14); double servoDeg(double servo_value); void touchGrab(); //タッチセンサ反応したらサーボ掴む float deg_c_grab = 11000; float deg_c_open = 8300; ////データ記録関連///// double now_deg=0; double now_count=0; ///////////////// ROS ///////////////// DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); float arm_turn=0,arm_deg=0,arm_rad=0; void cArmCallback(const geometry_msgs::Point &arm_state) { arm_turn = arm_state.x; //左回転=1、右回転=2 arm_deg = arm_state.y; //目標角度 } ros::NodeHandle nh; ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback); int main() { int mode = 0; myled1 = 0; //目標角度以上離れている:点灯 myled2 = 0; //通信で欲しい値が送られているとき:点灯 myled3 = 0; //state2に入ったとき←基本不要 myled4 = 0; //サーボを掴む→点灯.離す→消灯 ///////// ROS ///////// nh.getHardware()->setBaud(115200); nh.initNode(); nh.subscribe(sub); timer.reset(); timer.start(); motor.setEquation(0.0523,0.0148,0.0459,-0.0341); //Excel"omega_controll"の図より //////////////////////////////////////// /*arm_turn = 2; //左回転=1、右回転=2 arm_deg = -90; //目標角度*/ /////////////////////////////////////// while (1) { nh.spinOnce(); //ROS if(timer.read()>DELTA_T) { if(arm_turn==2&&arm_deg==-10) { //動作確認(通信)用 myled2 = 1; } arm_rad = degToRad(arm_deg); target_deg = arm_deg; target_rad = arm_rad; if(state == 0) { //トグルスイッチONでstate=1へ. ///トグルスイッチ1////マイコン方向 if(toggle1 == 0) { //トグルスイッチ1(mbed側)ONで初start wait_us(50); if(toggle1 == 0) { state++; } } } if(state == 1) { //回転方向の変数"arm_turn"に1か2が代入されたらstate=2へ. if(arm_turn == 1) { turn_deg = 90; state++; } else if(arm_turn == 2) { turn_deg = -90; state++; } turn_rad = degToRad(turn_deg); } else if(state == 2) { //サーボを離してそのまますぐstate=3へ. servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す state++; } else if (state == 3) {//タッチセンサがハンドルに反応したらサーボで掴んでstate=4へ. myled3 = 1; if(fabs(now_deg-turn_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す myled1 = 1; //LED1 点灯 motor.AcOmega(turn_rad); //速度式角度制御 } else { //目標角度に達したら停止 myled1=0; //LED1 消灯 motor.stop(); } touchGrab(); } else if (state == 4) {//目標角度を設定してそのまますぐstate=5へ. myled3 = 0; if(mode == 0){ target_rad = arm_rad; target_deg = arm_deg; } else if(mode == 1){ target_deg = 45; target_rad = degToRad(target_deg); } else if(mode == 2){ target_deg = -45; target_rad = degToRad(target_deg); } state++; } else if (state == 5) {//目標角度に近づいたら停止してサーボを離す. if(fabs(now_deg-target_deg) > 0.5) { //現在角度と目標角度が0.5°以上離れている間回す myled1 = 1; //LED1 点灯 motor.AcOmega(target_rad); //速度式角度制御 } else { //目標角度に達したら停止 myled1 = 0; //LED1 消灯 motor.stop(); //servo_c.set_degree(0,servoDeg(deg_c_open)); //servo_c離す myled4 = 0; if(mode == 0){ mode = 1; } else{ mode = 3-mode; } state = 4; /////////////////////連続して動かす時はここのコメントアウト外す. } } amt.rewriteCount(); amt.calOmega(); now_deg = amt.getDeg(); now_count=amt.getCount(); timer.reset(); } wait_us(10); } } ///////////////// 関数宣言部分 ////////////////////// double servoDeg(double servo_value) { return (servo_value - 3500)*270/(11500 - 3500); } double degToRad(double x) { return x*M_PI/180; } void touchGrab() { if(ts_c1 == 0) { //タッチセンサ反応したらハンドルを掴む wait_us(50); if(ts_c1 == 0) { servo_c.set_degree(0,servoDeg(deg_c_grab)); //servo_c掴む state++; myled4=1; //LED4 点灯 } } }