test_ultrasonic - mitsui

Users » tossapon2944 » Code » test_ultrasonic

toss tongbai / Mbed 2 deprecated test_ultrasonic

mitsui

Dependencies: PwmIn mbed ros_lib_melodic

Files at this revision

API Documentation at this revision

Revision 0:d2a396fcd7dd, committed 2021-11-01

Comitter:: tossapon2944
Date:: Mon Nov 01 10:48:47 2021 +0000
Commit message:: dd

Changed in this revision

diff -r 000000000000 -r d2a396fcd7dd PwmIn.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PwmIn.lib	Mon Nov 01 10:48:47 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/PwmIn/#6d68eb9b6bbb

diff -r 000000000000 -r d2a396fcd7dd main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 01 10:48:47 2021 +0000
@@ -0,0 +1,447 @@
+#include "mbed.h"
+#include <ros.h>
+#include <ros/time.h>
+#include "PwmIn.h"
+#include <std_msgs/Float32.h>
+#include <geometry_msgs/Point.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Vector3.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Int32MultiArray.h>
+#include <std_msgs/Int16MultiArray.h>
+#include <std_msgs/Int8.h>
+#include <geometry_msgs/Twist.h>
+#include <actionlib_msgs/GoalStatusArray.h>
+
+// Blinking rate in milliseconds
+#define BLINKING_RATE_MS  500
+
+ros::NodeHandle nh;
+
+DigitalIn button01(PF_9);
+DigitalIn button02(PF_7);
+DigitalIn button03(PF_8);
+DigitalIn button04(PE_3);
+DigitalIn button05(PE_6);
+DigitalIn button06(PE_5);
+DigitalIn button07(PE_4);
+DigitalIn button08(PE_2);
+DigitalIn button09(PD_7);
+DigitalIn button10(PG_2);
+DigitalIn button11(PG_3);
+DigitalIn button12(PD_2);
+DigitalIn button13(PF_3);
+
+DigitalOut out01(PF_15);
+DigitalOut out02(PF_14);
+DigitalOut out03(PF_13);
+DigitalOut out04(PE_10);
+DigitalOut out05(PB_2);
+DigitalOut out06(PD_13);
+DigitalOut out07(PD_12);
+DigitalOut out08(PE_15);
+DigitalOut out09(PE_7);
+DigitalOut out10(PE_8);
+DigitalOut out11(PB_15);
+DigitalOut out12(PB_8);
+DigitalOut out13(PB_9);
+DigitalOut out14(PD_14);
+DigitalOut out15(PB_3);
+DigitalOut out16(PB_13);
+DigitalOut out17(PB_4);
+//DigitalOut out18(PF_4);
+//DigitalOut out19(PF_3);
+
+//PIN OF ULTRASONIC
+DigitalOut trigPin(PC_6);
+PwmIn echo01(PB_10);
+PwmIn echo02(PE_12);
+PwmIn echo03(PE_14);
+PwmIn echo04(PD_15);
+PwmIn echo05(PE_13);
+PwmIn echo06(PE_11);
+
+bool press =false;
+bool msgs = false;
+
+float current_time;
+float previous_time;
+
+int st_app = 99;
+int st_alm;
+int st_mov;
+int st_bty;
+int st_drp;
+int st_brk;
+int pre_button = 0;
+//int led[10];
+
+std_msgs::Int8 btnros;
+std_msgs::Float32MultiArray distance1;
+
+void st_callback(const std_msgs::Int16MultiArray& st_data)
+{
+    st_app = st_data.data[0];
+    st_alm = st_data.data[1];
+    st_mov = st_data.data[2];
+    st_bty = st_data.data[3];
+    st_brk = st_data.data[4];
+   
+    //nh.loginfo("st_msg received ");
+}
+
+ros::Subscriber<std_msgs::Int16MultiArray> st_sub("sub_state",&st_callback);
+ros::Publisher btn_pub("/pub_button",&btnros);
+ros::Publisher ultra_pub("/ultra_raw",&distance1);
+
+Timer t_main;
+Timer t_pub;
+Timer t_press;
+Timer t_botton;
+int alarm =0;
+
+void onbutton()
+{         
+    if(!button01)
+    {
+        btnros.data = 1;
+        alarm = 1;
+        press = true;
+    }
+    else if(!button02)
+    {
+       btnros.data =2; 
+       alarm = 2;
+       press = true;
+    }
+    else if(!button03)
+    {
+        btnros.data =3;
+        alarm = 3;
+        press = true;
+    }
+    else if(!button04)
+    {
+        btnros.data =4;
+        alarm = 4;
+        press = true; 
+    }
+    else if(!button05)
+    {
+        btnros.data =5;
+        alarm = 5; 
+        press = true;
+    }
+    else if(!button06)
+    {
+        btnros.data =6;
+        alarm = 6;
+        press = true;
+    }
+    else if(!button07)
+    {
+        btnros.data =7;
+        alarm = 7;
+        press = true;
+    }
+    else if(!button08)
+    {
+        btnros.data =8;
+        alarm = 8;
+        press = true;
+    }
+    else if(!button09)
+    {
+        btnros.data =9;
+        alarm = 9;
+        press = true;
+    }
+    else if(!button10)
+    {
+        btnros.data =10;
+        alarm = 10;
+        press = true;
+    }
+    else if(!button11)
+    {
+        btnros.data=11;
+        alarm = 11;
+        press = true;
+    }
+    else if(!button12)
+    {
+        btnros.data=12;// reset alam 
+        alarm = 12;
+        press = true;
+        
+    }
+    else if(!button13)
+    {
+        btnros.data=13;// reset alam 
+        press = true;
+    }
+       
+        //nh.loginfo("in press btn");
+        pre_button = btnros.data;
+    
+}
+
+void zero()
+{
+    out01 = 1;  //station 1
+    out02 = 1;  //station 2
+    out03 = 1;  //station 3
+    out04 = 1;  //station 4
+    out05 = 1;  //station 5
+    out06 = 1;  //station 6
+    out07 = 1;  //station 7
+    out08 = 1;  //station 8
+    out09 = 1;  //station home
+    out10 = 1;  //station change
+    //out11 = 1;  //station drop point
+    out16 = 1;  //station drop point
+}
+    
+void nucleo_to_PLC()
+{
+    if(st_app == 99) //zero
+    {
+        zero();
+    }
+    else if(st_app == 0) //home
+    {
+        zero();
+        out09 = 0;
+    }
+    else if(st_app == 1) //station 1
+    {
+        zero();
+        out01 = 0; 
+    }
+    else if(st_app == 2) //station 2
+    {
+        zero();
+        out02 = 0; 
+    }
+    else if(st_app == 3) //station 3
+    {
+        zero();
+        out03 = 0; 
+    }
+    else if(st_app == 4) //station 4
+    {
+        zero();
+        out04 = 0;
+    }
+    else if(st_app == 5) //station 5
+    {
+        zero();
+        out05 = 0; 
+    }
+    else if(st_app == 6) //station 6
+    {
+        zero();
+        out06 = 0;
+    }
+    else if(st_app == 7) //station 7
+    {
+        zero();
+        out07 = 0; 
+    }
+    else if(st_app == 8) //station 8
+    {
+        zero();
+        out08 = 0;
+    }
+    else if(st_app == 9) //change station
+    {
+        zero();
+        out10 = 0;
+    }
+    else if(st_app == 10) //drop point
+    {
+        zero();
+        out16 = 0;
+    }
+    /////////////////////////////////
+    if(st_alm == 0 &&  alarm == 12)
+    {
+        out11 = 1; 
+        out14 = 1; 
+    }
+    else if(st_alm == 0 || st_alm == 3 && alarm != 12)
+    {
+        out11 = 0;  //robot ready
+        out14 = 1;
+    }
+    else if(st_alm == 99)
+    {
+        out11= 1;
+        out14 = 0;  //robot alarm
+    }
+    //////////////////////////////////
+    if(st_mov == 0)
+    {
+        out12 = 1;
+        out13 = 1;
+    }
+    else if(st_mov == 1)
+    {
+        out12 = 0;  //robot forword
+        out13 = 1;
+    }
+    else if(st_mov == 2)
+    {
+        out12 = 1;
+        out13 = 0;  //robot backword
+    }
+    //////////////////////////////////
+    if(st_bty == 0)
+    {
+        out15 = 1;  //battery status
+    }
+    else if(st_bty == 1)
+    {
+        out15 = 0;  //battery status (low)
+    }
+    else if(st_bty == 2)
+    {
+        out15 = 1;  //battery status (full)
+    }
+    //////////////////////////////////
+    if(st_brk == 0)
+    {
+        out17 = 1;  //normal break
+        //out18 = 1;
+    }
+    else if(st_brk == 1)
+    {
+        out17 = 1;  //not break
+    }
+    else if(st_brk == 3)
+    {
+        out17 = 0;  //break
+    }
+}
+
+void ultrasonic()
+{
+    float duration[6];
+    float points[6];
+    trigPin = false;                       // trig PIN
+    wait_us(2);
+    trigPin = true;
+    wait_us(10);
+    trigPin = false;    
+        
+    duration[0] = echo01.pulsewidth();         
+    //duration[1] = echo02.pulsewidth();          
+    //duration[2] = echo03.pulsewidth();          
+    duration[3] = echo04.pulsewidth(); 
+    //duration[4] = echo05.pulsewidth();    
+    duration[5] = echo06.pulsewidth();             
+        
+    points[0] = duration[0]/ 0.00560f;
+    //points[1] = duration[1]/ 0.00560f;
+    //points[2] = duration[2]/ 0.00560f;
+    points[3] = duration[3]/ 0.00560f;
+    //points[4] = duration[4]/ 0.00560f;
+    points[5] = duration[5]/ 0.00560f;
+    
+    distance1.data[0]= points[0];
+    //distance1.data[1]= points[1];
+    //distance1.data[2]= points[2];
+    distance1.data[1]= points[3];
+    //distance1.data[4]= points[4];
+    distance1.data[2]= points[5];
+}    
+
+//void st_robot()
+//{
+//}    
+
+int main()
+{
+    // Initialise the digital pin LED1 as an output
+    nh.initNode();
+    //nh.subscribe(btn_sub);
+    nh.subscribe(st_sub);
+    nh.advertise(btn_pub);
+    nh.advertise(ultra_pub);
+    nh.getHardware()->setBaud(57600);
+    
+    //int current_data = 0;
+    //int last_data = 0;
+    
+    button01.mode(PullUp);
+    button02.mode(PullUp);
+    button03.mode(PullUp);
+    button04.mode(PullUp);
+    button05.mode(PullUp);
+    button06.mode(PullUp);
+    button07.mode(PullUp);
+    button08.mode(PullUp);
+    button09.mode(PullUp);
+    button10.mode(PullUp);
+    button11.mode(PullUp);
+    button12.mode(PullUp);
+    button13.mode(PullUp);
+    
+    out01 = 1;
+    out02 = 1;
+    out03 = 1;
+    out04 = 1;
+    out05 = 1;
+    out06 = 1;
+    out07 = 1;
+    out08 = 1;
+    out09 = 1;
+    out10 = 1;
+    out11 = 1;
+    out12 = 1;
+    out13 = 1;
+    out14 = 1;
+    out15 = 1;
+    out16 = 1;
+    //out17=0;
+    //out18=0;
+    //out19=0;
+    distance1.layout.dim = (std_msgs::MultiArrayDimension*)
+    malloc(sizeof(std_msgs::MultiArrayDimension)*2);
+    distance1.layout.dim[0].label = "height1";
+    distance1.layout.dim[0].size =4;
+    distance1.layout.dim[0].stride = 1; 
+    distance1.layout.data_offset = 0;
+    distance1.data = (float *)malloc(sizeof(float)*8);
+    distance1.data_length = 3;
+    
+    t_main.start();
+    t_pub.start();
+    t_press.start();
+    
+    while (true)
+    {
+        if(t_main.read() >= 0.05f)
+        {
+            onbutton();
+            ultrasonic();
+            //btn_pub.publish(&btnros);
+            ultra_pub.publish(&distance1);
+            nucleo_to_PLC();
+            t_main.reset();
+        }
+     
+        if(press)
+        {  
+           if(t_pub.read() >= 0.5f){
+              btn_pub.publish(&btnros);
+              press = false;
+              t_pub.reset();
+            }
+        }
+        
+        //nucleo_to_PLC();
+        nh.spinOnce();
+    }
+    
+}

diff -r 000000000000 -r d2a396fcd7dd mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Nov 01 10:48:47 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

diff -r 000000000000 -r d2a396fcd7dd resources/official_armmbed_example_badge.png
Binary file resources/official_armmbed_example_badge.png has changed

diff -r 000000000000 -r d2a396fcd7dd ros_lib_melodic.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_melodic.lib	Mon Nov 01 10:48:47 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/garyservin/code/ros_lib_melodic/#da82487f547e

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	01 Nov 2021
Imports:	1
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	3
Followers:	1

PwmIn.lib	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
mbed.bld	Show annotated file Show diff for this revision Revisions of this file
resources/official_armmbed_example_badge.png	Show annotated file Show diff for this revision Revisions of this file
ros_lib_melodic.lib	Show annotated file Show diff for this revision Revisions of this file

Revision 0:d2a396fcd7dd, committed 2021-11-01

Changed in this revision

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning