NHK2020-m2-2-ros_2_25

ロボステ6期 » Code » NHK2020-m2-2-ros_2_25

ロボステ6期 / Mbed 2 deprecated NHK2020-m2-2-ros_2_25

Dependencies: mbed SpeedController___2_25 ros_lib_kinetic

Revision 1:9d6e9312dab1, committed 2020-02-25

Comitter:: yuki0701
Date:: Tue Feb 25 01:27:07 2020 +0000
Parent:: 0:c38fec6e4afd
Commit message:: a

Changed in this revision

--- a/SpeedController.lib	Sat Nov 16 06:29:02 2019 +0000
+++ b/SpeedController.lib	Tue Feb 25 01:27:07 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/ROBOSTEP_LIBRARY/code/SpeedController/#b1708b8819f1
+https://os.mbed.com/teams/2019-11/code/SpeedController___2_25/#b3e2ec75a4d7

--- a/can/can.cpp	Sat Nov 16 06:29:02 2019 +0000
+++ b/can/can.cpp	Tue Feb 25 01:27:07 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)50*(short)((msg.data[0]<<8) | msg.data[1]);
-            //mt_out2 = 1/(double)50*(short)((msg.data[2]<<8) | msg.data[3]);
-            mt_out1 = 1/(double)200*(short)((msg.data[4]<<8) | msg.data[5]);
-            mt_out2 = 1/(double)200*(short)((msg.data[6]<<8) | msg.data[7]);
-            //debug_printf("usw_data1 = %f\n\r",usw_data1);
-            printf("mt_out3 = %f,mt_out4 = %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)50*(short)((msg.data[0]<<8) | msg.data[1]);
+//            //mt_out2 = 1/(double)50*(short)((msg.data[2]<<8) | msg.data[3]);
+//            mt_out1 = 1/(double)200*(short)((msg.data[4]<<8) | msg.data[5]);
+//            mt_out2 = 1/(double)200*(short)((msg.data[6]<<8) | msg.data[7]);
+//            //debug_printf("usw_data1 = %f\n\r",usw_data1);
+//            printf("mt_out3 = %f,mt_out4 = %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:29:02 2019 +0000
+++ b/can/can.h	Tue Feb 25 01:27:07 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:29:02 2019 +0000
+++ b/main.cpp	Tue Feb 25 01:27:07 2020 +0000
@@ -1,72 +1,203 @@
-//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_2",&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(p13,p14,RESOLUTION);
+Ec4multi EC_2(p15,p16,RESOLUTION);
 //Ec4multi EC_1(p15,p16,RESOLUTION);
 //Ec4multi EC_2(p17,p18,RESOLUTION);
-Ec4multi EC_1(p15,p16,RESOLUTION);
-Ec4multi EC_2(p17,p18,RESOLUTION);
 
+SpeedControl motor_1(p21,p22,50,EC_1);
+SpeedControl motor_2(p24,p23,50,EC_2);
 //SpeedControl motor_1(p21,p22,50,EC_1);
-//SpeedControl motor_2(p23,p24,50,EC_2);
-SpeedControl motor_1(p22,p21,50,EC_1);
-SpeedControl motor_2(p23,p24,50,EC_2);
+//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 &mt2){
+    mt_out1 = (double)mt2.x/(double)4;
+    mt_out2 = (double)mt2.y/(double)4;
+    gb1 = (double)mt2.z;
+    gb2 = (double)mt2.w;
+}
+
+ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_mt2", &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,3,EC_1.getOmega());
     if(rotate_2==0) motor_2.stop();
-    else motor_2.Sc(mt_out2);
+    else motor_2.Sc(MT02,4,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.05,0.0013,0.04);    //求めたC,Dの値を設定
+    motor_2.setEquation(0.0013,0.055,0.0013,0.047);    //求めたC,Dの値を設定
     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
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+////CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
+//#include "mbed.h"
+//#include "EC.h" //Encoderライブラリをインクルード
+//#include "SpeedController.h"    //SpeedControlライブラリをインクルード
+//#include "can.h"
+//#define RESOLUTION 512  //分解能
+//#define BASIC_SPEED 40   //動確用
+////#define TEST_DUTY 0.3   //動確用
+//
+//
+////Ec4multi EC_1(p15,p16,RESOLUTION);
+////Ec4multi EC_2(p17,p18,RESOLUTION);
+//Ec4multi EC_1(p15,p16,RESOLUTION);
+//Ec4multi EC_2(p17,p18,RESOLUTION);
+//
+////SpeedControl motor_1(p21,p22,50,EC_1);
+////SpeedControl motor_2(p23,p24,50,EC_2);
+//SpeedControl motor_1(p22,p21,50,EC_1);
+//SpeedControl motor_2(p23,p24,50,EC_2);
+//
+//Ticker motor_tick;  //角速度計算用ticker
+//Serial pc(USBTX,USBRX);
+//
+//Timer timer;
+//
+//
+//int j=0;
+//double rotate_1=40,rotate_2=40;
+////double motor1_omega[1000]= {},motor2_omega[1000]= {},time_when[1000];
+////int motor1_count[1000]= {},motor2_count[1000]= {};
+//
+//void CalOmega()
+//{
+//    //motor1_count[j]=EC_1.getCount();
+//    //motor2_count[j]=EC_2.getCount();
+//    EC_1.calOmega();
+//    EC_2.calOmega();
+//    //motor1_omega[j]=EC_1.getOmega();
+//    //motor2_omega[j]=EC_2.getOmega();
+////    time_when[j]=EC_1.timer_.read();
+//    if(rotate_1==0) motor_1.stop();
+//    else motor_1.Sc(mt_out1);
+//    if(rotate_2==0) motor_2.stop();
+//    else motor_2.Sc(mt_out2);
+//
+//    j++;
+//}
+//int main()
+//{
+//    UserLoopSetting_can();
+//    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.setPDparam(0,0.0);  //PIDの係数を設定
+//    motor_2.setPDparam(0,0.0);  //PIDの係数を設定
+//    motor_tick.attach(&CalOmega,0.05);
+//
+//    int k = 0;
+//    
+//    while(1) {
+//        // rotate_1 = mt_out1;
+////        rotate_2 = mt_out2;
+//    
+//        if(k>100){
+//            printf("\nloop\n\n\r");
+//            k=0;
+//        }
+//        k++;
+//    }
+//    printf("finish\n\r");
+//}
\ 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:27:07 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f

Repository toolbox

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	25 Feb 2020
Imports:	0
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	3
Followers:	11

SpeedController.lib	Show annotated file Show diff for this revision Revisions of this file
can/can.cpp	Show annotated file Show diff for this revision Revisions of this file
can/can.h	Show annotated file Show diff for this revision Revisions of this file
main.cpp	Show annotated file Show diff for this revision Revisions of this file
ros_lib_kinetic.lib	Show annotated file Show diff for this revision Revisions of this file

Important changes to repositories hosted on mbed.com

Revision 1:9d6e9312dab1, committed 2020-02-25

Changed in this revision

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

Revision 1:9d6e9312dab1, committed 2020-02-25

Changed in this revision

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning