main.ino 8.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294
  1. //author:corvin
  2. //date:2016.11.18
  3. //description: connect all kinds of sensors in arduino sensor extens shield
  4. //create /sensor/lightValue topic to publish light sensor data;
  5. //create /sensor/temperature topic to publish temperature sensor data;
  6. //create /sensor/humidity topic to publish humidity sensor data;
  7. //create /sensor/gasSensor topic to publish gas sensor data
  8. //create /sensor/bodyDetect topic to publish body sensor data
  9. //create /sensor/IRValue topic to publish IR sensor data
  10. //create /sensor/touchValue topic to publish touch sensor data
  11. //create /sensor/topUltraSound topic to publish left ultraSound data
  12. //create /sensor/downUltraSound topic to publish right ultraSound data
  13. //How to check result in machine:
  14. //(1)start "roscore" in host;
  15. //(2)"rosrun rosserial_python serial_node.py /dev/ttyACM0 _baud:=57600"
  16. //(3)"rostopic echo /sensor/lightValue" --show light sensor data
  17. //(4)"rostopic echo /sensor/temperature" --show temperature sensor data
  18. //(5)"rostopic echo /sensor/humidity" --show humidity sensor data
  19. //(6)"rostopic echo /sensor/gasSensor" --show gas sensor data
  20. //(7)"rostopic echo /sensor/bodyDetect" --show body sensor data,0:no detect body,
  21. // --1:front detected,2:left detected,3:right detected
  22. //(8)"rostopic echo /sensor/IRValue" --show IR value
  23. //(9)"rostopic echo /sensor/touchValue" --show touch pose
  24. #include <ros.h>
  25. #include <dht.h>
  26. #include <std_msgs/String.h>
  27. #include <std_msgs/Byte.h>
  28. #include <std_msgs/Float32.h>
  29. #include <sensor_msgs/Range.h>
  30. #define DETECTBODY 1
  31. #define lightSensorPin 6 //light pin is Analog A6
  32. #define gasSensorPin 7 //gas sensor in Analog pin A7
  33. #define FontIRPin 9 //front IR sensor pin
  34. #define DHT22_PIN 53 //tempreture and humidity sensor pin
  35. #define FBodySensorPin 14 //front body sensor
  36. #define LBodySensorPin 15 //left body sensor
  37. #define RBodySensorPin 16 //right body sensor
  38. #define BBodySensorPin 17 //behind body sensor
  39. #define LeftTouchPin 42 //left touch
  40. #define RightTouchPin 41 //right touch
  41. #define topTrigPin 8 //top ultraSonic trig pin
  42. #define downTrigPin 9 //down ultraSonic trig pin
  43. #define topEchoPin 29 //top ultraSonic echo pin
  44. #define downEchoPin 28 //down ultraSonic echo pin
  45. #define TOPULTRASONIC 0x11 //top ultraSonic index
  46. #define DOWNULTRASONIC 0x22 //down ultrasonic index
  47. dht DHT;
  48. std_msgs::String light_msg;
  49. std_msgs::String tempe_msg;
  50. std_msgs::String humidity_msg;
  51. std_msgs::String gas_msg;
  52. std_msgs::Byte body_msg;
  53. std_msgs::Float32 IR_msg;
  54. std_msgs::Byte touch_msg;
  55. sensor_msgs::Range top_range_msg;
  56. sensor_msgs::Range down_range_msg;
  57. ros::Publisher pub_light("/sensor/lightValue", &light_msg);
  58. ros::Publisher pub_temp("/sensor/temperature", &tempe_msg);
  59. ros::Publisher pub_humidity("/sensor/humidity", &humidity_msg);
  60. ros::Publisher pub_gas("/sensor/gasValue", &gas_msg);
  61. ros::Publisher pub_body("/sensor/bodyDetect", &body_msg);
  62. ros::Publisher pub_IR("/sensor/IRValue", &IR_msg);
  63. ros::Publisher pub_touch("/sensor/touchValue", &touch_msg);
  64. ros::Publisher pub_topRange("/sensor/topUltraSound", &top_range_msg);
  65. ros::Publisher pub_downRange("/sensor/downUltraSound", &down_range_msg);
  66. ros::NodeHandle nh;
  67. char temp[5];
  68. char humi[5];
  69. char lightStr[5];
  70. char gasStr[6];
  71. char ultraSonic_frameid[] = "/ultraSonic_ranger";
  72. //init function
  73. void setup() {
  74. setupPinMode();
  75. setupAdvertisePub();
  76. setupUltraSonicMsg();
  77. }
  78. void loop() {
  79. pubTempeHumiSensorData();
  80. pubIRData();
  81. pubLightSensorData();
  82. pubGasSensorData();
  83. pubBodyDetectData();
  84. pubTouchSensorData();
  85. pubUltraSonicData(TOPULTRASONIC); //publish top UltraSonic value
  86. pubUltraSonicData(DOWNULTRASONIC); //publish down UltraSonic value
  87. nh.spinOnce();
  88. }
  89. void setupPinMode()
  90. {
  91. pinMode(FBodySensorPin, INPUT);
  92. pinMode(LBodySensorPin, INPUT);
  93. pinMode(RBodySensorPin, INPUT);
  94. pinMode(BBodySensorPin, INPUT);
  95. pinMode(LeftTouchPin, INPUT);
  96. pinMode(RightTouchPin, INPUT);
  97. pinMode(topTrigPin, OUTPUT);
  98. pinMode(topEchoPin, INPUT);
  99. pinMode(downTrigPin, OUTPUT);
  100. pinMode(downEchoPin, INPUT);
  101. }
  102. void setupAdvertisePub()
  103. {
  104. nh.initNode();
  105. nh.advertise(pub_light);
  106. nh.advertise(pub_temp);
  107. nh.advertise(pub_humidity);
  108. nh.advertise(pub_gas);
  109. nh.advertise(pub_body);
  110. nh.advertise(pub_IR);
  111. nh.advertise(pub_touch);
  112. nh.advertise(pub_topRange);
  113. nh.advertise(pub_downRange);
  114. }
  115. void setupUltraSonicMsg()
  116. {
  117. top_range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  118. top_range_msg.header.frame_id = ultraSonic_frameid;
  119. top_range_msg.field_of_view = 0.1;
  120. top_range_msg.min_range = 0.25;
  121. top_range_msg.max_range = 4.5;
  122. down_range_msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
  123. down_range_msg.header.frame_id = ultraSonic_frameid;
  124. down_range_msg.field_of_view = 0.1;
  125. down_range_msg.min_range = 0.25;
  126. down_range_msg.max_range = 4.5;
  127. }
  128. void pubTempeHumiSensorData()
  129. {
  130. delay(1000);
  131. int chk = DHT.read22(DHT22_PIN); //read sensor data
  132. tempe_msg.data = dtostrf(DHT.temperature, 3, 1, temp);
  133. pub_temp.publish(&tempe_msg);
  134. humidity_msg.data = dtostrf(DHT.humidity, 3, 1, humi);
  135. pub_humidity.publish(&humidity_msg);
  136. }
  137. void pubLightSensorData()
  138. {
  139. int val = analogRead(lightSensorPin); //connect grayscale sensor to Analog
  140. light_msg.data = itostr(lightStr, val);
  141. pub_light.publish(&light_msg);
  142. }
  143. void pubGasSensorData()
  144. {
  145. int data = analogRead(gasSensorPin);
  146. gas_msg.data = itostr(gasStr, data);
  147. pub_gas.publish(&gas_msg);
  148. }
  149. void pubTouchSensorData()
  150. {
  151. //get touch position
  152. byte pose = 0;
  153. if (digitalRead(LeftTouchPin) && digitalRead(RightTouchPin))
  154. {
  155. pose = 3;
  156. }
  157. else if (digitalRead(RightTouchPin))
  158. {
  159. pose = 2;
  160. }
  161. else if (digitalRead(LeftTouchPin))
  162. {
  163. pose = 1;
  164. }
  165. else
  166. {
  167. pose = 0;
  168. }
  169. if (pose != 0)
  170. {
  171. //publish msg to topic,when detect touch will decrease delay
  172. touch_msg.data = pose;
  173. pub_touch.publish(&touch_msg);
  174. }
  175. if (pose == 0)
  176. {
  177. delay(1000);
  178. }
  179. }
  180. //publish body detect sensor data
  181. void pubBodyDetectData()
  182. {
  183. byte result = 0;
  184. byte Astate = digitalRead(FBodySensorPin);
  185. byte Bstate = digitalRead(LBodySensorPin);
  186. byte Cstate = digitalRead(RBodySensorPin);
  187. byte Dstate = digitalRead(BBodySensorPin);
  188. if (DETECTBODY == Astate) //detect front body
  189. {
  190. result = 1;
  191. }
  192. else if (DETECTBODY == Bstate) //detect left body
  193. {
  194. result = 2;
  195. }
  196. else if (DETECTBODY == Cstate) //detect right body
  197. {
  198. result = 3;
  199. }
  200. else if (DETECTBODY == Dstate) //detect behind body
  201. {
  202. result = 4;
  203. }
  204. else //no detect body
  205. {
  206. result = 0;
  207. }
  208. //publish msg to topic
  209. if (result != 0)
  210. {
  211. body_msg.data = result;
  212. pub_body.publish(&body_msg);
  213. }
  214. }
  215. //get IR value
  216. void pubIRData()
  217. {
  218. float volts = analogRead(FontIRPin) * 0.0048828125;
  219. float distance = 65 * pow(volts, -1.10);
  220. IR_msg.data = distance;
  221. pub_IR.publish(&IR_msg);
  222. }
  223. void pubUltraSonicData(int flag)
  224. {
  225. if (flag == TOPULTRASONIC) //top ultraSonic
  226. {
  227. digitalWrite(topTrigPin, LOW); // Set the trigger pin to low for 2uS
  228. delayMicroseconds(2);
  229. digitalWrite(topTrigPin, HIGH); // Send a 10uS high to trigger ranging
  230. delayMicroseconds(10);
  231. digitalWrite(topTrigPin, LOW); // Send pin low again
  232. top_range_msg.range = getRange(topEchoPin);
  233. top_range_msg.header.stamp = nh.now();
  234. pub_topRange.publish(&top_range_msg);
  235. }
  236. else //down ultraSonic
  237. {
  238. digitalWrite(downTrigPin, LOW); // Set the trigger pin to low for 2uS
  239. delayMicroseconds(2);
  240. digitalWrite(downTrigPin, HIGH); // Send a 10uS high to trigger ranging
  241. delayMicroseconds(10);
  242. digitalWrite(downTrigPin, LOW); // Send pin low again
  243. down_range_msg.range = getRange(downEchoPin);
  244. down_range_msg.header.stamp = nh.now();
  245. pub_downRange.publish(&down_range_msg);
  246. }
  247. }
  248. float getRange(int pin_num)
  249. {
  250. float distance;
  251. distance = pulseIn(pin_num, HIGH, 26000); // Read in times pulse
  252. distance = distance / 5800.00;
  253. return distance;
  254. }
  255. /*convert int to string*/
  256. char* itostr(char *str, int i)
  257. {
  258. sprintf(str, "%d", i);
  259. return str;
  260. }