자율주행/ROS기반 자율주행모형차 개발
(9) 아두이노 펌웨어
Hagler
2019. 3. 5. 06:13
|
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
|
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>
ros::NodeHandle nh;
Servo servo;
int dir = 8;
int pwm = 11;
int val = 0;
int angle = 100;
void servo_cb( const std_msgs::UInt16MultiArray& cmd_msg){
val = cmd_msg.data[0]; //set servo angle, should be from 0-180
angle = cmd_msg.data[1];
servo.write(angle);
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
}
ros::Subscriber<std_msgs::UInt16MultiArray> sub("control", servo_cb);
void setup(){
pinMode(13, OUTPUT);
pinMode(dir,OUTPUT);
pinMode(pwm,OUTPUT);
nh.initNode();
nh.subscribe(sub);
servo.attach(6);
}
void loop(){
digitalWrite(dir,LOW);
analogWrite(pwm,val);
nh.spinOnce();
delay(1);
}
|
cs |