pi_esp_communicator/servo_test.ino at main · kentaokamoto/pi_esp_communicator · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <sensor_msgs/msg/imu.h>
#include <sensor_msgs/msg/magnetic_field.h>
#include <std_msgs/msg/int32.h>

#include <Arduino_LSM9DS1.h>
#include <Servo.h>

rcl_publisher_t publisher_imu;
rcl_publisher_t publisher_mag;

rcl_subscription_t pwm_subscriber;

rclc_executor_t executor;
rclc_executor_t motor_executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

sensor_msgs__msg__Imu msg_imu;
sensor_msgs__msg__MagneticField msg_mag;
std_msgs__msg__Int32 msg_pwm;

int motor0_pin = 14;
Servo motor0;

void motor_callback(const void * msgin)
{
  const std_msgs__msg__Int32 * msg_pwm = (const std_msgs__msg__Int32 *)msgin;
  //analogWrite(MOTOR0_PIN, 1000);

}

void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
  float ax, ay, az, gx, gy, gz, mx, my, mz;
  RCLC_UNUSED(last_call_time);
  if (timer != NULL){
    if(IMU.accelerationAvailable())
    {
      IMU.readAcceleration(ax,ay,az);
    }
    if(IMU.gyroscopeAvailable())
    {
      IMU.readGyroscope(gx, gy, gz);
    }
    if(IMU.magneticFieldAvailable())
    {
      IMU.readMagneticField(mx, my, mz);
    }

    //ax = ax*9.797;
    //ay = ay*9.797;
    //az = az*9.797;
    msg_imu.linear_acceleration.x = ax;
    msg_imu.linear_acceleration.y = ay;
    msg_imu.linear_acceleration.z = az;

    gx = gx*3.142/180;
    gy = gy*3.142/180;
    gz = gz*3.142/180;
    msg_imu.angular_velocity.x = gx;
    msg_imu.angular_velocity.y = gy;
    msg_imu.angular_velocity.z = gz;

    msg_mag.magnetic_field.x = mx;
    msg_mag.magnetic_field.y = my;
    msg_mag.magnetic_field.z = mz;

    rcl_publish(&publisher_imu, (const void *) &msg_imu, NULL);
    rcl_publish(&publisher_mag, (const void *) &msg_mag, NULL);
  }
}

void setup() {
  set_microros_transports();
  delay(5000);
  IMU.begin();
  motor0.attach(motor0_pin);
  delay(1000);
  motor0.writeMicroseconds(1000);
  delay(1000);
  motor0.writeMicroseconds(2000);
  delay(1000);
  //analogWriteResolution(12);
  //analogWriteFrequency(MOTOR0_PIN, 250);
  //delay(5000);
  //analogWrite(MOTOR0_PIN, 4095);
  //delay(1000);
  //analogWrite(MOTOR0_PIN, 0);
  ///////////////////////////////////////////////////////////////////////////////////////////////////
  allocator = rcl_get_default_allocator();

  // create init_options
  rclc_support_init(&support, 0, NULL, &allocator);

  // create node
  rclc_node_init_default(&node, "pi_esp_communicator", "", &support);

  // create publisher
  rclc_publisher_init_default(
    &publisher_imu,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
    "/copto/imu");

  rclc_publisher_init_default(
    &publisher_mag,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, MagneticField),
    "/copto/mag");


  // create subscriber
  rclc_subscription_init_default(
    &pwm_subscriber,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "/copto/motor0_pwm");

  // create timer,
  const unsigned int timer_timeout = 100;
  rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback);

  // create executor
  executor = rclc_executor_get_zero_initialized_executor();
  rclc_executor_init(&executor, &support.context, 2, &allocator);
  rclc_executor_add_timer(&executor, &timer);

  motor_executor = rclc_executor_get_zero_initialized_executor();
  rclc_executor_init(&motor_executor, &support.context, 2, &allocator);
  rclc_executor_add_subscription(&motor_executor, &pwm_subscriber, &msg_pwm, &motor_callback, ON_NEW_DATA);
}

void loop() {
  rclc_executor_spin(&executor);
  delay(100);
  rclc_executor_spin(&motor_executor);
  delay(100);
  motor0.writeMicroseconds(1000);
  delay(100);
}