駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

main.cpp@3:a45557a0dcb8, 2018-12-11 (annotated)

Committer:
nakedt555
Date:
Tue Dec 11 17:51:47 2018 +0000
Revision:
3:a45557a0dcb8
Parent:
2:086272a2da1c
Child:
4:cf1a4e503974
12/10iikanji

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nakedt555 3:a45557a0dcb8 1 #include <mbed.h>
nakedt555 3:a45557a0dcb8 2 #include <vector>
nakedt555 0:10f626cf3ec4 3
nakedt555 0:10f626cf3ec4 4 #include "myRos.h"
nakedt555 3:a45557a0dcb8 5 #include "myCan.h"
nakedt555 1:bdd17feaa4ce 6 #include <ros.h>
nakedt555 1:bdd17feaa4ce 7 #include <ros/time.h>
nakedt555 1:bdd17feaa4ce 8 #include <std_msgs/Empty.h>
nakedt555 1:bdd17feaa4ce 9 #include "odom.h"
nakedt555 1:bdd17feaa4ce 10
nakedt555 2:086272a2da1c 11 //prototype
nakedt555 2:086272a2da1c 12 void odom_update_callback();
nakedt555 2:086272a2da1c 13
nakedt555 1:bdd17feaa4ce 14 //Create subscliber
nakedt555 1:bdd17feaa4ce 15 ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb);
nakedt555 0:10f626cf3ec4 16
nakedt555 2:086272a2da1c 17 //Initialize GPIO
nakedt555 0:10f626cf3ec4 18 DigitalOut myled(LED1);
nakedt555 0:10f626cf3ec4 19 DigitalOut led0(PC_0);
nakedt555 0:10f626cf3ec4 20 DigitalOut led1(PC_1);
nakedt555 0:10f626cf3ec4 21 DigitalOut led2(PB_0);
nakedt555 0:10f626cf3ec4 22 DigitalIn mysw(PC_13);
nakedt555 0:10f626cf3ec4 23
nakedt555 3:a45557a0dcb8 24 void led0_toggle(void) { led0 = !led0; }
nakedt555 3:a45557a0dcb8 25 void led1_toggle(void) { led1 = !led1; }
nakedt555 3:a45557a0dcb8 26 void led2_toggle(void) { led2 = !led2; }
nakedt555 3:a45557a0dcb8 27
nakedt555 3:a45557a0dcb8 28 //Create Odom instance
nakedt555 2:086272a2da1c 29 Odom odom;
nakedt555 2:086272a2da1c 30
nakedt555 0:10f626cf3ec4 31 int main() {
nakedt555 1:bdd17feaa4ce 32 //Initialize TIM
nakedt555 1:bdd17feaa4ce 33 Timer GetTick;
nakedt555 1:bdd17feaa4ce 34 GetTick.start();
nakedt555 1:bdd17feaa4ce 35 uint32_t debug_ts = 0;
nakedt555 1:bdd17feaa4ce 36 uint32_t ros_loop_ts = 0;
nakedt555 2:086272a2da1c 37
nakedt555 1:bdd17feaa4ce 38 //Initialize ROS
nakedt555 3:a45557a0dcb8 39 My_Ros my_ros(&odom);
nakedt555 1:bdd17feaa4ce 40 my_ros.set_subscliber(ack_sub);
nakedt555 3:a45557a0dcb8 41
nakedt555 3:a45557a0dcb8 42 My_Can my_can(&odom);
nakedt555 3:a45557a0dcb8 43 my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl);
nakedt555 3:a45557a0dcb8 44 my_can.led_toggle_attach(&led1_toggle);
nakedt555 3:a45557a0dcb8 45
nakedt555 3:a45557a0dcb8 46 std::vector<Odom_Abstract*> odom_container;
nakedt555 3:a45557a0dcb8 47 odom_container.push_back(&my_ros);
nakedt555 3:a45557a0dcb8 48 odom_container.push_back(&my_can);
nakedt555 1:bdd17feaa4ce 49
nakedt555 0:10f626cf3ec4 50 while(1) {
nakedt555 1:bdd17feaa4ce 51 //For debug
nakedt555 1:bdd17feaa4ce 52 if(debug_ts <= GetTick.read_ms()){
nakedt555 3:a45557a0dcb8 53 debug_ts = GetTick.read_ms() + 100;
nakedt555 3:a45557a0dcb8 54 led0_toggle();
nakedt555 1:bdd17feaa4ce 55 }
nakedt555 1:bdd17feaa4ce 56
nakedt555 3:a45557a0dcb8 57 //For odom loop
nakedt555 1:bdd17feaa4ce 58 if(ros_loop_ts <= GetTick.read_ms()){
nakedt555 1:bdd17feaa4ce 59 ros_loop_ts = GetTick.read_ms() + 10;
nakedt555 3:a45557a0dcb8 60 for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){
nakedt555 3:a45557a0dcb8 61 (*it)->loop();
nakedt555 3:a45557a0dcb8 62 }
nakedt555 1:bdd17feaa4ce 63 }
nakedt555 2:086272a2da1c 64
nakedt555 3:a45557a0dcb8 65 if(mysw == 0){
nakedt555 3:a45557a0dcb8 66 while(mysw == 0);
nakedt555 3:a45557a0dcb8 67 my_can.test();
nakedt555 3:a45557a0dcb8 68 }
nakedt555 2:086272a2da1c 69
nakedt555 2:086272a2da1c 70 //角度計算用:ポーリングに置いとく
nakedt555 2:086272a2da1c 71 odom.update_angle();
nakedt555 0:10f626cf3ec4 72 }
nakedt555 2:086272a2da1c 73 }