駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

main.cpp@2:086272a2da1c, 2018-12-07 (annotated)

Committer:
nakedt555
Date:
Fri Dec 07 22:34:31 2018 +0000
Revision:
2:086272a2da1c
Parent:
1:bdd17feaa4ce
Child:
3:a45557a0dcb8
12/08 work log;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nakedt555 0:10f626cf3ec4 1 #include "mbed.h"
nakedt555 0:10f626cf3ec4 2
nakedt555 0:10f626cf3ec4 3 #include "myRos.h"
nakedt555 1:bdd17feaa4ce 4 #include <ros.h>
nakedt555 1:bdd17feaa4ce 5 #include <ros/time.h>
nakedt555 1:bdd17feaa4ce 6 #include <std_msgs/Empty.h>
nakedt555 1:bdd17feaa4ce 7
nakedt555 1:bdd17feaa4ce 8 #include "odom.h"
nakedt555 1:bdd17feaa4ce 9
nakedt555 2:086272a2da1c 10 //prototype
nakedt555 2:086272a2da1c 11 void odom_update_callback();
nakedt555 2:086272a2da1c 12
nakedt555 1:bdd17feaa4ce 13 //Create subscliber
nakedt555 1:bdd17feaa4ce 14 ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb);
nakedt555 0:10f626cf3ec4 15
nakedt555 2:086272a2da1c 16 //Initialize GPIO
nakedt555 0:10f626cf3ec4 17 DigitalOut myled(LED1);
nakedt555 0:10f626cf3ec4 18 DigitalOut led0(PC_0);
nakedt555 0:10f626cf3ec4 19 DigitalOut led1(PC_1);
nakedt555 0:10f626cf3ec4 20 DigitalOut led2(PB_0);
nakedt555 0:10f626cf3ec4 21 DigitalIn mysw(PC_13);
nakedt555 0:10f626cf3ec4 22
nakedt555 2:086272a2da1c 23 //Initialize ODOM
nakedt555 2:086272a2da1c 24 Odom odom;
nakedt555 2:086272a2da1c 25
nakedt555 0:10f626cf3ec4 26 int main() {
nakedt555 1:bdd17feaa4ce 27 //Initialize TIM
nakedt555 1:bdd17feaa4ce 28 Timer GetTick;
nakedt555 1:bdd17feaa4ce 29 GetTick.start();
nakedt555 1:bdd17feaa4ce 30 uint32_t debug_ts = 0;
nakedt555 1:bdd17feaa4ce 31 uint32_t ros_loop_ts = 0;
nakedt555 2:086272a2da1c 32
nakedt555 2:086272a2da1c 33 //Set TIM IT Callback
nakedt555 2:086272a2da1c 34 Ticker odom_tick;
nakedt555 2:086272a2da1c 35 odom_tick.attach(&odom_update_callback, 0.02);
nakedt555 1:bdd17feaa4ce 36
nakedt555 1:bdd17feaa4ce 37 //Initialize ROS
nakedt555 1:bdd17feaa4ce 38 My_Ros my_ros;
nakedt555 1:bdd17feaa4ce 39 my_ros.set_subscliber(ack_sub);
nakedt555 1:bdd17feaa4ce 40
nakedt555 0:10f626cf3ec4 41 while(1) {
nakedt555 1:bdd17feaa4ce 42 //For debug
nakedt555 1:bdd17feaa4ce 43 if(debug_ts <= GetTick.read_ms()){
nakedt555 1:bdd17feaa4ce 44 debug_ts = GetTick.read_ms() + 20;
nakedt555 1:bdd17feaa4ce 45 led0 = !led0;
nakedt555 1:bdd17feaa4ce 46 }
nakedt555 1:bdd17feaa4ce 47
nakedt555 1:bdd17feaa4ce 48 //For myRos loop
nakedt555 1:bdd17feaa4ce 49 if(ros_loop_ts <= GetTick.read_ms()){
nakedt555 1:bdd17feaa4ce 50 ros_loop_ts = GetTick.read_ms() + 10;
nakedt555 1:bdd17feaa4ce 51 my_ros.loop();
nakedt555 1:bdd17feaa4ce 52 }
nakedt555 2:086272a2da1c 53
nakedt555 2:086272a2da1c 54
nakedt555 2:086272a2da1c 55 //角度計算用:ポーリングに置いとく
nakedt555 2:086272a2da1c 56 odom.update_angle();
nakedt555 0:10f626cf3ec4 57 }
nakedt555 0:10f626cf3ec4 58 }
nakedt555 2:086272a2da1c 59
nakedt555 2:086272a2da1c 60 void odom_update_callback(){
nakedt555 2:086272a2da1c 61 odom.update_it_cb();
nakedt555 2:086272a2da1c 62 }