Revision 5:fe91d31db27e, committed 2020-02-25

Comitter:

yuki0701

Date:

Tue Feb 25 01:23:14 2020 +0000

Parent:

4:46c10d34af27

Commit message:

a

Changed in this revision

--- a/SpeedController.lib	Sat Nov 16 06:28:18 2019 +0000
+++ b/SpeedController.lib	Tue Feb 25 01:23:14 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/SpeedController/#b1708b8819f1
+https://os.mbed.com/teams/2019-11/code/SpeedController2_25/#1f6b35a255f2

--- a/can/can.cpp	Sat Nov 16 06:28:18 2019 +0000
+++ b/can/can.cpp	Tue Feb 25 01:23:14 2020 +0000
@@ -1,103 +1,103 @@
-#include "mbed.h"
-#include "can.h"
-
-CAN can1(p30,p29);
-Ticker can_ticker;  //can用ticker
-
-DigitalOut cansend_led(LED1);  //cansend -> on
-DigitalOut canread_led(LED2);  //canread -> on
-
-double mt_out1=0.0,mt_out2=0.0;
-
-int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
-
-void can_readsend()
-{
-    CANMessage msg;
-
-
-    if(can1.read(msg)) {
-        //printf(" CAN success\n\r");
-        if(msg.id == 4) {
-            mt_out1 = 1/(double)200*(short)((msg.data[0]<<8) | msg.data[1]);
-            mt_out2 = 1/(double)200*(short)((msg.data[2]<<8) | msg.data[3]);
-            //mt_out1 = 1/(double)50*(short)((msg.data[4]<<8) | msg.data[5]);
-            //mt_out2 = 1/(double)50*(short)((msg.data[6]<<8) | msg.data[7]);
-            //debug_printf("usw_data1 = %f\n\r",usw_data1);
-            printf("mt_out1 = %f,mt_out2 = %f\n\r",mt_out1,mt_out2);
-        }
-      //  if(msg.id == 7){
-//            mt_out2 = 1/(double)50*(short)((msg.data[0]<<8) | msg.data[1]);
-//            printf("id = 5 mt_out2 = %f, data[0] = %d, data[1] = %d\n\r",mt_out2,msg.data[0],msg.data[1]);
+//#include "mbed.h"
+//#include "can.h"
+//
+//CAN can1(p30,p29);
+//Ticker can_ticker;  //can用ticker
+//
+//DigitalOut cansend_led(LED1);  //cansend -> on
+//DigitalOut canread_led(LED2);  //canread -> on
+//
+//double mt_out1=0.0,mt_out2=0.0;
+//
+//int t1_r=0, T1=0; //動作番号（受け取った値、送信する値（int型））
+//
+//void can_readsend()
+//{
+//    CANMessage msg;
+//
+//
+//    if(can1.read(msg)) {
+//        //printf(" CAN success\n\r");
+//        if(msg.id == 4) {
+//            mt_out1 = 1/(double)200*(short)((msg.data[0]<<8) | msg.data[1]);
+//            mt_out2 = 1/(double)200*(short)((msg.data[2]<<8) | msg.data[3]);
+//            //mt_out1 = 1/(double)50*(short)((msg.data[4]<<8) | msg.data[5]);
+//            //mt_out2 = 1/(double)50*(short)((msg.data[6]<<8) | msg.data[7]);
+//            //debug_printf("usw_data1 = %f\n\r",usw_data1);
+//            printf("mt_out1 = %f,mt_out2 = %f\n\r",mt_out1,mt_out2);
 //        }
-
-    } else {
-        canread_led = 0;
-        printf("fale\n\r");
-    }
-
-
-    // can_ashileddata[1] = T1;  //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
-
-//    can_ashileddata2[0] = m1 >> 8;
-//    can_ashileddata2[1] = m1 &255;
+//      //  if(msg.id == 7){
+////            mt_out2 = 1/(double)50*(short)((msg.data[0]<<8) | msg.data[1]);
+////            printf("id = 5 mt_out2 = %f, data[0] = %d, data[1] = %d\n\r",mt_out2,msg.data[0],msg.data[1]);
+////        }
 //
-//    can_ashileddata3[0] = m1 >> 8;
-//    can_ashileddata3[1] = m1 &255;
-//
-//    can_ashileddata4[0] = m1 >> 8;
-//    can_ashileddata4[1] = m1 &255;
-//
-//    can_ashileddata5[0] = m1 >> 8;
-//    can_ashileddata5[1] = m1 &255;
-//
-//    if(can1.write(CANMessage(4,can_ashileddata2,2))) {  //IDを4にして送信
-//        cansend_led = 1;
 //    } else {
-//        cansend_led = 0;
+//        canread_led = 0;
+//        printf("fale\n\r");
 //    }
 //
-//    if(can1.write(CANMessage(4,can_ashileddata3,2))) {  //IDを4にして送信
-//        cansend_led = 1;
-//    } else {
-//        cansend_led = 0;
-//    }
+//
+//    // can_ashileddata[1] = T1;  //動作番号（id節約のため、can_ashileddata[]と一緒に送る）
 //
-//    if(can1.write(CANMessage(4,can_ashileddata4,2))) {  //IDを4にして送信
-//        cansend_led = 1;
-//    } else {
-//        cansend_led = 0;
-//    }
+////    can_ashileddata2[0] = m1 >> 8;
+////    can_ashileddata2[1] = m1 &255;
+////
+////    can_ashileddata3[0] = m1 >> 8;
+////    can_ashileddata3[1] = m1 &255;
+////
+////    can_ashileddata4[0] = m1 >> 8;
+////    can_ashileddata4[1] = m1 &255;
+////
+////    can_ashileddata5[0] = m1 >> 8;
+////    can_ashileddata5[1] = m1 &255;
+////
+////    if(can1.write(CANMessage(4,can_ashileddata2,2))) {  //IDを4にして送信
+////        cansend_led = 1;
+////    } else {
+////        cansend_led = 0;
+////    }
+////
+////    if(can1.write(CANMessage(4,can_ashileddata3,2))) {  //IDを4にして送信
+////        cansend_led = 1;
+////    } else {
+////        cansend_led = 0;
+////    }
+////
+////    if(can1.write(CANMessage(4,can_ashileddata4,2))) {  //IDを4にして送信
+////        cansend_led = 1;
+////    } else {
+////        cansend_led = 0;
+////    }
+////
+////    if(can1.write(CANMessage(4,can_ashileddata5,2))) {  //IDを4にして送信
+////        cansend_led = 1;
+////    } else {
+////        cansend_led = 0;
+////    }
 //
-//    if(can1.write(CANMessage(4,can_ashileddata5,2))) {  //IDを4にして送信
-//        cansend_led = 1;
-//    } else {
-//        cansend_led = 0;
+//    /*if(t1_r > T1) {
+//        T1 = t1_r;
+//    }*/
+//}
+//
+//void can_start()
+//{
+//
+//    while(1) {
+//
+//        CANMessage msg;
+//
+////        debug_printf("wait\n\r");
+//        printf("wait\n\r");
+//        wait(0.1);
+//        if(can1.read(msg)) {
+//            break;
+//        }
 //    }
-
-    /*if(t1_r > T1) {
-        T1 = t1_r;
-    }*/
-}
-
-void can_start()
-{
-
-    while(1) {
-
-        CANMessage msg;
-
-//        debug_printf("wait\n\r");
-        printf("wait\n\r");
-        wait(0.1);
-        if(can1.read(msg)) {
-            break;
-        }
-    }
-}
-
-void UserLoopSetting_can()
-{
-    can1.frequency(1000000);
-    can_ticker.attach(&can_readsend,0.01);  //遅かったら早める
-}
\ No newline at end of file
+//}
+//
+//void UserLoopSetting_can()
+//{
+//    can1.frequency(1000000);
+//    can_ticker.attach(&can_readsend,0.01);  //遅かったら早める
+//}
\ No newline at end of file

--- a/can/can.h	Sat Nov 16 06:28:18 2019 +0000
+++ b/can/can.h	Tue Feb 25 01:23:14 2020 +0000
@@ -1,14 +1,14 @@
-#ifndef HARUROBO2019_CAN
-#define HARUROBO2019_CAN
-
-extern int T1;
-
-void can_readsend();
-
-void can_start();
-
-void UserLoopSetting_can();
-
-extern double mt_out1,mt_out2;
-
-#endif
\ No newline at end of file
+//#ifndef HARUROBO2019_CAN
+//#define HARUROBO2019_CAN
+//
+//extern int T1;
+//
+//void can_readsend();
+//
+//void can_start();
+//
+//void UserLoopSetting_can();
+//
+////extern double mt_out1,mt_out2;
+//
+//#endif
\ No newline at end of file

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

--- a/mbed.bld	Sat Nov 16 06:28:18 2019 +0000
+++ b/mbed.bld	Tue Feb 25 01:23:14 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/mbed_official/code/mbed/builds/1c57384330a6
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_kinetic.lib	Tue Feb 25 01:23:14 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f