Diff: main.cpp

Revision:

5:fe91d31db27e

Parent:

4:46c10d34af27

--- a/main.cpp	Sat Nov 16 06:28:18 2019 +0000
+++ b/main.cpp	Tue Feb 25 01:23:14 2020 +0000
@@ -1,15 +1,126 @@
+//#include "mbed.h"
+//#include "math.h"
+//#include "EC.h" //Encoderライブラリをインクルード
+//#include "SpeedController.h"    //SpeedControlライブラリをインクルード
+//
+//#include <ros.h>
+//#include <geometry_msgs/Point.h>
+//#include <geometry_msgs/Pose.h>
+//
+//ros::NodeHandle nh;
+//geometry_msgs::Point posi_xyr;
+////geometry_msgs::Quaternion usw_4info;
+//
+//ros::Publisher pub_xyr("/check_1",&posi_xyr);
+////ros::Publisher pub_usw("/mbed_main2",&usw_4info);
+//
+//Ticker cm_pc;
+//#define PI 3.141592
+//#define RESOLUTION 512  //分解能
+//#define BASIC_SPEED 40   //動確
+//
+//Ec4multi EC_1(p16,p15,RESOLUTION);
+//Ec4multi EC_2(p18,p17,RESOLUTION);
+//SpeedControl motor_1(p21,p22,50,EC_1);
+//SpeedControl motor_2(p24,p23,50,EC_2);
+//
+//Ticker motor_tick;  //角速度計算用ticker
+//Timer timer;
+//
+//int i = 0;
+//int j=0;
+////Serial pc(USBTX,USBRX);
+//double rotate_1=40,rotate_2=40;
+//
+//double gb1, gb2;
+//double mt_out1,mt_out2;
+//
+////void messageCallback(const geometry_msgs::Quaternion &mt1){
+////    mt_out1 = mt1.x;
+////    mt_out2 = mt1.y;
+////    gb1 = mt1.z;
+////    gb2 = mt1.w;
+////}
+//
+////ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt1", &messageCallback);
+//
+//void cm_to_pc(){
+//    
+//    
+//    posi_xyr.x = 1;
+//    posi_xyr.y = 2;
+//    posi_xyr.z = 3;
+//    
+//    
+//    pub_xyr.publish(&posi_xyr);
+//    
+//}
+//
+//
+////-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
+////DigitalOut cansend_led(LED1);  //cansend -> on                //can.cpp
+////DigitalOut canread_led(LED2);  //canread -> on                //can.cpp
+////DigitalOut debug_led(LED3);    //maxon debug programme -> on  //maxonsetting.cpp
+//
+////////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
+//
+//int main()
+//{
+//    nh.getHardware()->setBaud(115200);
+//    nh.initNode();
+//    nh.advertise(pub_xyr);
+////    nh.advertise(pub_usw);
+//    
+//    cm_pc.attach(&cm_to_pc,0.01);
+//
+////    UserLoopSetting_sensor();
+//    
+//    while(1) {
+//
+//        nh.spinOnce();
+//        wait(0.01);
+//
+//    }
+//
+//}
+//
+//
+
+
+
+
+
+
+////
+
 //CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
+
 #include "mbed.h"
+#include "math.h"
+#include <ros.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+#include <std_msgs/String.h>
 #include "EC.h" //Encoderライブラリをインクルード
 #include "SpeedController.h"    //SpeedControlライブラリをインクルード
-#include "can.h"
+
+ros::NodeHandle nh;
+geometry_msgs::Point check_com;
+
+ros::Publisher pub_check("/check_1",&check_com);
+
+
+int i = 0;
+
+double mt_out1,mt_out2;
+
 #define RESOLUTION 512  //分解能
 #define BASIC_SPEED 40   //動確用
 //#define TEST_DUTY 0.3   //動確用
 
 
-Ec4multi EC_1(p16,p15,RESOLUTION);
-Ec4multi EC_2(p18,p17,RESOLUTION);
+Ec4multi EC_1(p13,p14,RESOLUTION);
+Ec4multi EC_2(p15,p16,RESOLUTION);
 //Ec4multi EC_1(p15,p16,RESOLUTION);
 //Ec4multi EC_2(p17,p18,RESOLUTION);
 
@@ -19,54 +130,85 @@
 //SpeedControl motor_2(p24,p23,50,EC_2);
 
 Ticker motor_tick;  //角速度計算用ticker
-Serial pc(USBTX,USBRX);
+//Serial pc(USBTX,USBRX); //このコメントを外したままROSとの通信をしようとすると、なぜか通信ができないので、コメントアウトしています。
 
 Timer timer;
 
 
 int j=0;
 double rotate_1=40,rotate_2=40;
+double gO_1;
+double gO_2;
+double MT01=0,MT02=0;
 //double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
 //int motor1_count[1000]= {},motor2_count[1000]= {};
+double gb1, gb2;
+
+void messageCallback(const geometry_msgs::Quaternion &mt1){
+    mt_out1 = (double)mt1.x/(double)4;
+    mt_out2 = (double)mt1.y/(double)4;
+    gb1 = (double)mt1.z;
+    gb2 = (double)mt1.w;
+}
+
+ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt1", &messageCallback);
 
 void CalOmega()
 {
+    
+    check_com.x = mt_out1;
+    check_com.y = mt_out2;
+    check_com.z = 12;
+    
+    pub_check.publish(&check_com);
+    
+    //printf("check\n\r");
+    
+    
     //motor1_count[j]=EC_1.getCount();
     //motor2_count[j]=EC_2.getCount();
     EC_1.calOmega();
     EC_2.calOmega();
+    gO_1=EC_1.getOmega();
+    gO_2=EC_2.getOmega();    
     //motor1_omega[j]=EC_1.getOmega();
     //motor2_omega[j]=EC_2.getOmega();
 //    time_when[j]=EC_1.timer_.read();
+    MT01=mt_out1;
+    MT02=mt_out2;
     if(rotate_1==0) motor_1.stop();
-    else motor_1.Sc(mt_out1);
+    else motor_1.Sc(MT01,1,EC_1.getOmega());
     if(rotate_2==0) motor_2.stop();
-    else motor_2.Sc(mt_out2);
+    else motor_2.Sc(MT02,2,EC_2.getOmega());
 
-    j++;
+    j++; 
 }
+
 int main()
 {
-    UserLoopSetting_can();
+    
+    nh.getHardware()->setBaud(115200);
+    nh.initNode();
+    nh.advertise(pub_check);
+    nh.subscribe(sub);
+    
     motor_1.period_us(50);
     motor_2.period_us(50);
-    motor_1.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
-    motor_2.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
+    //motor_1.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
+    //motor_2.setEquation(0.0429,0.05,0.0429,0.05);    //求めたC,Dの値を設定
+    motor_1.setEquation(0.0013,0.0525,0.0013,0.045);    //求めたC,Dの値を設定
+    //motor_2.setEquation(0.0013,0.03,0.0013,0.035);    //求めたC,Dの値を設定
+    motor_2.setEquation(0.0013,0.05,0.0013,0.055);    //求めたC,Dの値を設定//no arm ver
     motor_1.setPDparam(0,0.0);  //PIDの係数を設定
     motor_2.setPDparam(0,0.0);  //PIDの係数を設定
-    motor_tick.attach(&CalOmega,0.05);
+    //motor_1.setPDparam(0.1,0.0);  //PIDの係数を設定
+    //motor_2.setPDparam(0.1,0.0);  //PIDの係数を設定
+    motor_tick.attach(&CalOmega,0.01);
 
-    int k = 0;
     
     while(1) {
-        // rotate_1 = mt_out1;
-//        rotate_2 = mt_out2;
-    
-        if(k>100){
-            printf("\nloop\n\n\r");
-            k=0;
-        }
-        k++;
+        nh.spinOnce();
+        wait(0.01);
     }
     printf("finish\n\r");
 }
\ No newline at end of file