Diff: main.cpp

Revision:

0:83b2c6a67cce

Child:

1:c0a3e4589a8f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 08 03:46:29 2021 +0000
@@ -0,0 +1,134 @@
+////c = center arm
+#include "mbed.h"
+#include "AMT21.h"
+#include "CalPID.h"
+#include "KondoServo.h"
+#include "MotorController.h"
+#include <stdlib.h>
+
+/////////////////               ROS               /////////////////
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Point.h>
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+float arm_num=0,arm_deg=0;
+void cArmCallback(const geometry_msgs::Point &arm_state)
+{
+    arm_num = arm_state.x;
+    arm_deg = arm_state.y;
+}
+ros::NodeHandle  nh;
+ros::Subscriber<geometry_msgs::Point> sub("/arm",&cArmCallback);
+
+/////////////////               宣言部分                  //////////////////////
+#ifndef M_PI
+#define M_PI 3.14159265359f
+#endif
+#define DELTA_T 0.01    //制御周期
+#define DUTY_MAX 0.07     //duty比の最大値
+#define OMEGA_MAX 0.6    //速度制御を利用した角度制御での角速度の最大値
+#define NUM_DATA 270
+
+Serial pc(USBTX, USBRX);
+////角度制御(アブソリュートエンコーダ)/////
+DigitalOut myled1(LED1);
+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.5,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 target_speed=0;
+double target_rad=0;
+double target_deg = 0;
+double radToDeg(double x);
+
+////先端アーム(サーボ)/////
+DigitalIn ts_c1(p6, PullUp);
+DigitalIn ts_c2(p7, PullUp);
+KondoServo servo_c(p13, p14);
+double servoDeg(double servo_value);
+double calServoDeg(double servo_value);     //360°の度数法にする
+int id_c1 = 0;
+float deg_c_grab = 9000;
+float deg_c_open = 3500;
+
+
+////データ記録まわり/////
+int angle_count=0;
+int omega_count=0;
+double now_deg=0;    //度数法
+double now_count=0;
+
+int main()
+{
+    myled1 = 0;     //アブソリュートエンコーダ用if文中で点灯
+    /////////       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"の図より
+
+//    target_rad = -(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）           //////////////////////////
+//    target_deg = radToDeg(target_rad);
+
+    while (1) {
+        ////    ROS     ////
+        nh.spinOnce();
+        if(arm_num==1&&arm_deg==60) {       //LEDは動作確認用
+            myled2 = !myled2;
+        }
+        if(arm_num == 1) {
+            myled3 = 1;
+        }
+        if(arm_deg == 60) {
+            myled4 = 1;
+        }
+        
+        ////タッチセンサ////
+        if(ts_c1 == 0) {      //タッチセンサ反応したらハンドルを掴む
+            wait_us(50);
+            if(ts_c1 == 0) {
+                servo_c.set_degree(0,servoDeg(4500));   //servo_c掴む
+            }
+        }
+
+        ////アブソリュートエンコーダ////
+        if(timer.read()>DELTA_T) {
+            if(fabs(now_deg-target_deg) > 0.5) {      //現在角度と目標角度が0.5°以上離れている間回す
+                myled1=1;   //LED2 点灯
+                motor.AcOmega(target_rad);   //速度式角度制御
+            } else { //目標角度に達したら停止
+                myled1=0;   //LED2 消灯
+                motor.stop();
+                servo_c.set_degree(0,servoDeg(1000));   //servo_c離す
+            }
+            amt.rewriteCount();
+            amt.calOmega();
+            amt.getOmega();
+            now_deg = amt.getDeg();    //度数法
+            now_count=amt.getCount();
+            motor.AcOmega(target_rad);
+
+//            pc.printf("now_deg = %f,%f\r\n", now_deg,now_count);       //現在角度取得 (確認以外ではコメントアウトしないとモーター回らない)////////////////////
+            timer.reset();
+        }
+        wait_us(10);
+    }
+}
+
+/////////////////               関数宣言部分                  //////////////////////
+double servoDeg(double servo_value)
+{
+    return (servo_value - 3500)*270/(11500 - 3500);
+}
+double radToDeg(double x)
+{
+    return x*180/M_PI;
+}
\ No newline at end of file