駿 高世 / Mbed 2 deprecated abu2019_mr1

aaa

Dependencies:   mbed BNO055_fusion Adafruit_GFX ros_lib_kinetic

main.cpp@12:f726eb78b54c, 2019-08-30 (annotated)

Committer:
nakedt555
Date:
Fri Aug 30 07:55:38 2019 +0000
Revision:
12:f726eb78b54c
Parent:
6:20a32baeff79 
aaa

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 4:cf1a4e503974 6 #include "myOled.h"
nakedt555 1:bdd17feaa4ce 7 #include <ros.h>
nakedt555 1:bdd17feaa4ce 8 #include <ros/time.h>
nakedt555 1:bdd17feaa4ce 9 #include <std_msgs/Empty.h>
nakedt555 4:cf1a4e503974 10 #include <geometry_msgs/Point.h>
nakedt555 1:bdd17feaa4ce 11 #include "odom.h"
nakedt555 1:bdd17feaa4ce 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 4:cf1a4e503974 15 ros::Subscriber<geometry_msgs::Point> drift_sub("nucleo/drift", &drift_sub_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 6:20a32baeff79 30 My_Can my_can(&odom);
nakedt555 6:20a32baeff79 31
nakedt555 6:20a32baeff79 32 void attach_can_cb(void){ my_can.receive_cb(); }
nakedt555 6:20a32baeff79 33
nakedt555 0:10f626cf3ec4 34 int main() {
nakedt555 1:bdd17feaa4ce 35 //Initialize TIM
nakedt555 1:bdd17feaa4ce 36 Timer GetTick;
nakedt555 1:bdd17feaa4ce 37 GetTick.start();
nakedt555 1:bdd17feaa4ce 38 uint32_t debug_ts = 0;
nakedt555 1:bdd17feaa4ce 39 uint32_t ros_loop_ts = 0;
nakedt555 2:086272a2da1c 40
nakedt555 1:bdd17feaa4ce 41 //Initialize ROS
nakedt555 3:a45557a0dcb8 42 My_Ros my_ros(&odom);
nakedt555 1:bdd17feaa4ce 43 my_ros.set_subscliber(ack_sub);
nakedt555 4:cf1a4e503974 44 my_ros.set_subscliber(drift_sub);
nakedt555 3:a45557a0dcb8 45
nakedt555 4:cf1a4e503974 46 //Initialize CAN
nakedt555 3:a45557a0dcb8 47 my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl);
nakedt555 3:a45557a0dcb8 48 my_can.led_toggle_attach(&led1_toggle);
nakedt555 6:20a32baeff79 49 my_can.can_rx_it_cb(&attach_can_cb);
nakedt555 3:a45557a0dcb8 50
nakedt555 4:cf1a4e503974 51 //Initialize OLED
nakedt555 4:cf1a4e503974 52 I2C i2c(PB_4, PA_8);
nakedt555 4:cf1a4e503974 53 My_Oled my_oled(&odom, i2c);
nakedt555 4:cf1a4e503974 54
nakedt555 3:a45557a0dcb8 55 std::vector<Odom_Abstract*> odom_container;
nakedt555 3:a45557a0dcb8 56 odom_container.push_back(&my_ros);
nakedt555 3:a45557a0dcb8 57 odom_container.push_back(&my_can);
nakedt555 4:cf1a4e503974 58 odom_container.push_back(&my_oled);
nakedt555 1:bdd17feaa4ce 59
nakedt555 0:10f626cf3ec4 60 while(1) {
nakedt555 1:bdd17feaa4ce 61 //For debug
nakedt555 1:bdd17feaa4ce 62 if(debug_ts <= GetTick.read_ms()){
nakedt555 3:a45557a0dcb8 63 debug_ts = GetTick.read_ms() + 100;
nakedt555 3:a45557a0dcb8 64 led0_toggle();
nakedt555 1:bdd17feaa4ce 65 }
nakedt555 1:bdd17feaa4ce 66
nakedt555 3:a45557a0dcb8 67 //For odom loop
nakedt555 1:bdd17feaa4ce 68 if(ros_loop_ts <= GetTick.read_ms()){
nakedt555 1:bdd17feaa4ce 69 ros_loop_ts = GetTick.read_ms() + 10;
nakedt555 3:a45557a0dcb8 70 for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){
nakedt555 3:a45557a0dcb8 71 (*it)->loop();
nakedt555 3:a45557a0dcb8 72 }
nakedt555 1:bdd17feaa4ce 73 }
nakedt555 2:086272a2da1c 74
nakedt555 4:cf1a4e503974 75 //Debug
nakedt555 3:a45557a0dcb8 76 if(mysw == 0){
nakedt555 3:a45557a0dcb8 77 while(mysw == 0);
nakedt555 3:a45557a0dcb8 78 my_can.test();
nakedt555 3:a45557a0dcb8 79 }
nakedt555 2:086272a2da1c 80
nakedt555 2:086272a2da1c 81 //角度計算用:ポーリングに置いとく
nakedt555 2:086272a2da1c 82 odom.update_angle();
nakedt555 0:10f626cf3ec4 83 }
nakedt555 2:086272a2da1c 84 }