Advertisement
Guest User

Untitled

a guest
Sep 23rd, 2017
54
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 19.38 KB | None | 0 0
  1. using System;//
  2. using System.Collections.Generic;
  3. using System.ComponentModel;
  4. using Microsoft.Ccr.Core;
  5. using Microsoft.Dss.Core.Attributes;
  6. using Microsoft.Dss.ServiceModel.Dssp;
  7. using Microsoft.Dss.ServiceModel.DsspServiceBase;
  8. using W3C.Soap;
  9. using submgr = Microsoft.Dss.Services.SubscriptionManager;
  10. using lightsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.LightSensor.Proxy;
  11. using touchsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.TouchSensor.Proxy;
  12. using buttons = Microsoft.Robotics.Services.Sample.Lego.Nxt.Buttons.Proxy;
  13. using drive = Microsoft.Robotics.Services.Sample.Lego.Nxt.Drive.Proxy;
  14. using colorv2 = Microsoft.Robotics.Services.Sample.HiTechnic.ColorV2.Proxy;
  15. using System.Timers;
  16.  
  17. namespace Lab4_203_02
  18. {
  19. [Contract(Contract.Identifier)]
  20. [DisplayName("Lab4_203_02")]
  21. [Description("Lab4_203_02 service (no description provided)")]
  22. class Lab4_203_02Service : DsspServiceBase
  23. {
  24.  
  25. bool spotlight = false; // flag to determine whether spotlight is on
  26. int red = 0;
  27. int green = 0;
  28. int blue = 0;
  29. int colournumber = 0;
  30. bool yellow1 = false;
  31. bool blue1 = false;
  32. bool blue2 = false;
  33. bool green1 = false;
  34. bool green2 = false;
  35. bool green3 = false;
  36. bool green4 = false;
  37. bool red1 = false;
  38. bool red2 = false;
  39. bool red3 = false;
  40. bool red4 = false;
  41. bool red5 = false;
  42. bool back = false;
  43.  
  44. public static System.Timers.Timer LeaveNode;
  45.  
  46.  
  47. /// <summary>
  48. /// Service state
  49. /// </summary>
  50. [ServiceState]
  51. Lab4_203_02State _state = new Lab4_203_02State();
  52.  
  53. /// <summary>
  54. /// Main service port
  55. /// </summary>
  56. [ServicePort("/Lab4_203_02", AllowMultipleInstances = true)]
  57. Lab4_203_02Operations _mainPort = new Lab4_203_02Operations();
  58.  
  59. [SubscriptionManagerPartner]
  60. submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();
  61.  
  62. /// <summary>
  63. /// NxtLightSensor partner
  64. /// </summary>
  65. [Partner("NxtLightSensor", Contract = lightsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
  66. lightsensor.LightSensorOperations _nxtLightSensorPort = new lightsensor.LightSensorOperations();
  67.  
  68. /// <summary>
  69. /// NxtTouchSensor partner
  70. /// </summary>
  71. [Partner("NxtTouchSensor", Contract = touchsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
  72. touchsensor.TouchSensorOperations _nxtTouchSensorPort = new touchsensor.TouchSensorOperations();
  73.  
  74. /// <summary>
  75. /// NxtButtons partner
  76. /// </summary>
  77. [Partner("NxtButtons", Contract = buttons.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
  78. buttons.ButtonOperations _nxtButtonsPort = new buttons.ButtonOperations();
  79.  
  80. /// <summary>
  81. /// NxtDrive partner
  82. /// </summary>
  83. [Partner("NxtDrive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
  84. drive.DriveOperations _nxtDrivePort = new drive.DriveOperations();
  85.  
  86. /// <summary>
  87. /// HiTechnicColorV2Sensor partner
  88. /// </summary>
  89. [Partner("HiTechnicColorV2Sensor", Contract = colorv2.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
  90. colorv2.ColorV2SensorOperations _hiTechnicColorV2SensorPort = new colorv2.ColorV2SensorOperations();
  91.  
  92. /// <summary>
  93. /// Service constructor
  94. /// </summary>
  95. public Lab4_203_02Service(DsspServiceCreationPort creationPort)
  96. : base(creationPort)
  97. {
  98. }
  99.  
  100. protected override void Start()
  101. {
  102. base.Start();
  103. touchsensor.TouchSensorOperations touchsensorNotificationPort = new touchsensor.TouchSensorOperations();
  104.  
  105. _nxtTouchSensorPort.Subscribe(touchsensorNotificationPort);
  106.  
  107. Activate(
  108. Arbiter.Receive<touchsensor.TouchSensorUpdate>
  109. (true, touchsensorNotificationPort, TouchSensorHandler));
  110.  
  111. lightsensor.LightSensorOperations lightsensorNotificationPort = new lightsensor.LightSensorOperations();
  112.  
  113. _nxtLightSensorPort.Subscribe(lightsensorNotificationPort);
  114.  
  115. Activate(
  116. Arbiter.Receive<lightsensor.Replace>
  117. (true, lightsensorNotificationPort, LightSensorHandler));
  118. buttons.ButtonOperations buttonNotificationPort = new buttons.ButtonOperations();
  119.  
  120. _nxtButtonsPort.Subscribe(buttonNotificationPort);
  121.  
  122. Activate(
  123. Arbiter.Receive<buttons.ButtonsUpdate>
  124. (true, buttonNotificationPort, ButtonPressHandler));
  125.  
  126. drive.DriveOperations driverNotificationPort = new drive.DriveOperations();
  127. _nxtDrivePort.Subscribe(driverNotificationPort);
  128.  
  129. Activate(
  130. Arbiter.Receive<buttons.ButtonsUpdate>
  131. (true, buttonNotificationPort, ButtonPressHandler));
  132.  
  133. colorv2.ColorV2SensorOperations hiTechnicColorV2SensorPort = new colorv2.ColorV2SensorOperations();
  134. _hiTechnicColorV2SensorPort.Subscribe(hiTechnicColorV2SensorPort);
  135.  
  136. Activate(
  137. Arbiter.Receive<colorv2.ColorV2SensorUpdate>
  138. (true, hiTechnicColorV2SensorPort, colorHandler));
  139.  
  140. }
  141.  
  142. [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
  143. public virtual IEnumerator<ITask> GetHandler(Get get)
  144. {
  145. get.ResponsePort.Post(_state);
  146. yield break;
  147. }
  148.  
  149. /// <summary>
  150. /// Handle TouchSensor Notifications
  151. /// </summary>
  152. /// <param name="notification">Update notification</param>
  153. ///
  154. // if touchsensor is pressed check if spotlight is true if so make false if false make true
  155. private void TouchSensorHandler(touchsensor.TouchSensorUpdate notification)
  156. {
  157. if (notification.Body.TouchSensorOn)
  158. {
  159. spotlight = !spotlight;
  160. _nxtLightSensorPort.Spotlight(spotlight);
  161.  
  162. //<------- MOVING METHOD CALLS------>
  163. //MovingLeft();
  164. //MovingRight30();
  165. //MovingLeft();
  166. //MovingSlowly();
  167. //Turn180();
  168.  
  169. //<------- END OF MOVING METHOD CALLS------>
  170.  
  171. //<------- COLOUR CHECK CALLS------>
  172. //isBlack();
  173. //isWhite();
  174. //isRed();
  175. //isGreen();
  176. //isBlue();
  177. //isYellow();
  178. //<------- MOVING METHOD CALLS------>
  179.  
  180.  
  181.  
  182. }
  183. }
  184.  
  185. public void colorHandler(colorv2.ColorV2SensorUpdate notification)
  186. {
  187. red = notification.Body.Red;
  188. green = notification.Body.Green;
  189. blue = notification.Body.Blue;
  190. colournumber = notification.Body.ColorNumber;
  191.  
  192. if (isWhite())
  193. {
  194. Response();
  195. }
  196. else if (isBlack())
  197. {
  198. Response();
  199. }
  200. else if (isYellow())// now at the only yellow
  201. {
  202. if (!yellow1)
  203. {
  204. findPaths(true);// check to see if there are any paths
  205. yellow1 = true; // take the right path
  206. }
  207. else
  208. {
  209. findPaths(false); // if you have already visited the yellow node then turn left
  210. }
  211. }
  212. else if (isBlue()) // if on blue node
  213. {
  214.  
  215. if (!blue1) // if blue 1 is false
  216. {
  217. findPaths(true); // turn right
  218. blue1 = true; //make blue 1 true
  219. }
  220. else if (back && !blue2)
  221. {
  222. findPaths(false); // if on way back then turn left (should this not be right)
  223. }
  224. else if (!back && blue2)
  225. {
  226. findPaths(true);
  227. }
  228. else if (back && !blue2)
  229. {
  230. findPaths(false);
  231. }
  232.  
  233. }
  234.  
  235. else if (isGreen()) // if on blue node
  236. {
  237.  
  238. if (!green1 && !back) // if blue 1 is false
  239. {
  240. findPaths(true); // since only one path go straight
  241. green1 = true; //make blue 1 true
  242. }
  243. else if (back && !green2)
  244. {
  245. findPaths(true); // take the right path
  246. }
  247. else if (!back && !green2)
  248. {
  249. findPaths(false);
  250. green2 = true;
  251. }
  252. else if (back && green2)
  253. {
  254. findPaths(false);// take the path on the right
  255. }
  256.  
  257. else if (!back && !green3)
  258. {
  259. findPaths(true);
  260. green3 = true; // if on way back then turn left (should this not be right)
  261. }
  262.  
  263. else if (back && green3)
  264. {
  265. findPaths(true);
  266. }
  267.  
  268. else if (!back && !green4)
  269. {
  270. findPaths(true);
  271. green4 = true; // if on way back then turn left (should this not be right)
  272. }
  273.  
  274. else if (back && green4)
  275. {
  276. findPaths(true);
  277. }
  278.  
  279. }
  280.  
  281.  
  282. else if (isRed()) // if on blue node
  283. {
  284. if (!red1 && !back)
  285. {
  286. red1 = true;
  287. Turn180();
  288. }
  289.  
  290. else if (red1 && back)
  291. {
  292. findPaths(true);// go straight
  293. }
  294.  
  295. else if (!red2 && !back)
  296. {
  297. red2 = true;
  298. Turn180();
  299. }
  300.  
  301. else if (red2 && back)
  302. {
  303. findPaths(true);
  304. }
  305.  
  306. else if (!red3 && !back)
  307. {
  308. red3 = true;
  309. Turn180();
  310. }
  311.  
  312. else if (red3 && back)
  313. {
  314. findPaths(true);
  315. }
  316.  
  317. else if (!red4 && !back)
  318. {
  319. red4 = true;
  320. Turn180();
  321. }
  322.  
  323. else if (red4 && back)
  324. {
  325. findPaths(true);
  326. }
  327.  
  328. else if (!red5 && !back)
  329. {
  330. red5 = true;
  331. Turn180();
  332. }
  333.  
  334. else if (red5 && back)
  335. {
  336. findPaths(true);
  337. }
  338. }
  339.  
  340. }
  341.  
  342. public void Turn180()
  343. {
  344. drive.SetDriveRequest request = new drive.SetDriveRequest(0.5, -0.5, 0);
  345. _nxtDrivePort.DriveDistance(request);
  346.  
  347. Console.WriteLine("in180");
  348. System.Threading.Thread.Sleep(500);
  349. }// check this
  350.  
  351. private void ButtonPressHandler(buttons.ButtonsUpdate notification)
  352. {
  353. if (notification.Body.PressedRight)// when the right button is pressed
  354. {
  355. Console.WriteLine("the bot should drive");
  356. // <--- Checking Line Correction code ----->
  357. Response();
  358. }
  359.  
  360. else if (notification.Body.PressedLeft)// left button is pressed
  361. {
  362.  
  363. Console.WriteLine("the bot should stop");
  364. _nxtDrivePort.DriveDistance(0, 0, 0); // set all values to stop the robot from moving
  365. }
  366.  
  367. } // right press drive, left button presss stop
  368.  
  369. private void LightSensorHandler(lightsensor.Replace notification)
  370. {
  371.  
  372. }
  373.  
  374. public void SubscribeHandler(Subscribe subscribe)
  375. {
  376. SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
  377. } // end of subscribe
  378.  
  379. public void Response()
  380. {
  381. if (isBlack())
  382. {
  383. MovingSlowly();
  384. }
  385.  
  386. if (isWhite())
  387. {
  388. System.Timers.Timer whiteTimer = new System.Timers.Timer();
  389. whiteTimer.Elapsed += new ElapsedEventHandler(checkLeft);
  390. whiteTimer.Interval = 1000;
  391.  
  392. MovingRight30();
  393.  
  394. whiteTimer.Enabled = true;
  395.  
  396. Console.WriteLine("timer enabled");
  397.  
  398. while (isBlack() == false)
  399. {
  400.  
  401. }
  402.  
  403. // if (isBlack) {
  404. whiteTimer.Enabled = false;
  405.  
  406. Console.WriteLine("timer disabled");
  407.  
  408. MovingSlowly();
  409. //}
  410. }
  411. }
  412.  
  413. public void checkLeft(object source, ElapsedEventArgs e)
  414. {
  415. Console.WriteLine("Timer elapsed");
  416. MovingLeft();
  417. }
  418.  
  419. public Boolean isBlack()
  420. {
  421. if (colournumber == 13 || colournumber == 14)
  422. {
  423. if (red <= 100 && blue <= 100 && green <= 100)
  424. {
  425. Console.WriteLine("On Black");
  426. return true;
  427.  
  428. }
  429. else return false;
  430. }
  431. else return false;
  432. }
  433.  
  434. public Boolean isWhite()
  435. {
  436. if (colournumber == 13 || colournumber == 14)
  437. {
  438. if (red >= 240 && blue >= 240 && green >= 240)
  439. {
  440. Console.WriteLine("On White");
  441. return true;
  442. }
  443. else return false;
  444. }
  445. else return false;
  446. }// print line finished
  447.  
  448. public Boolean isRed()
  449. {
  450. if (colournumber == 10)
  451. {
  452. Console.WriteLine(" Is red");
  453. return true;
  454. }
  455. return false;
  456. }// print line finished
  457.  
  458. public Boolean isYellow()
  459. {
  460. if (colournumber == 13 || colournumber == 14)
  461. {
  462. if (red <= 100 && blue <= 100 && green <= 100)
  463. {
  464. Console.WriteLine("Yellow");
  465. return true;
  466. }
  467. else return false;
  468. }
  469. else return false;
  470. }// print line is finished
  471.  
  472. public Boolean isGreen()
  473. {
  474. if (colournumber == 5)
  475. {
  476. Console.WriteLine("Green");
  477. return true;
  478. }
  479. else return false;
  480. }// print line is finished
  481.  
  482. public Boolean isBlue()
  483. {
  484. if (colournumber == 3)
  485. {
  486. Console.WriteLine("Blue");
  487. return true;
  488. }
  489. else return false;
  490. }// print line is finished
  491.  
  492. public Boolean isFinishGreen()
  493. {
  494. if (colournumber == 4)
  495. {
  496. Console.WriteLine(" Found EndPoint");
  497. return true;
  498. }
  499. else return false;
  500. }
  501.  
  502. public void MovingSlowly()
  503. {
  504. drive.SetDriveRequest request = new drive.SetDriveRequest(0.2, 0.2, 0);
  505. _nxtDrivePort.DriveDistance(request);
  506. Console.WriteLine(" Moving slowly");
  507. }// (0.2, 0.2, 0)
  508.  
  509. public void MovingLeft()// (0, 0.3, 35)
  510. {
  511. drive.SetDriveRequest request = new drive.SetDriveRequest(0, 0.3, 35);
  512.  
  513. _nxtDrivePort.DriveDistance(request);
  514. System.Threading.Thread.Sleep(500);
  515. Console.WriteLine("left");
  516. }
  517.  
  518. public void MovingRight30() // (0, .03, 60);// line following
  519. {
  520. drive.SetDriveRequest request = new drive.SetDriveRequest(0, .03, 60);
  521. _nxtDrivePort.DriveDistance(request);
  522. System.Threading.Thread.Sleep(500);
  523. Console.WriteLine(" Moving right 30");
  524. }
  525.  
  526. public void StopMoving()
  527. {
  528. drive.SetDriveRequest request = new drive.SetDriveRequest(0, 0, 0);
  529. _nxtDrivePort.DriveDistance(request);
  530. System.Threading.Thread.Sleep(500);
  531. Console.WriteLine("Stopped Moving");
  532. }// (0,0,0)
  533.  
  534. public void OutofNode() //moving out of node Should we enable a timer?
  535. {
  536.  
  537. LeaveNode = new System.Timers.Timer();
  538. LeaveNode.Elapsed += new ElapsedEventHandler(EndOfLeavingNode);
  539. LeaveNode.Interval = 1500; // milliseconds
  540. LeaveNode.Enabled = true;
  541. drive.SetDriveRequest request = new drive.SetDriveRequest(0.03, 0.03, 0);
  542. Response();
  543. Console.WriteLine("Out of Node");
  544. }
  545.  
  546. public void EndOfLeavingNode( object o , ElapsedEventArgs e)
  547. {
  548. drive.SetDriveRequest request = new drive.SetDriveRequest(0.00, 0.00, 00);
  549. }
  550.  
  551.  
  552. public void findPaths(bool turnRight) // find paths is called when you are on a node and you know. should hek for all possible paths out and decide on which path to take based on the boolean passed if boolean is true should take right path if false left.
  553. {
  554.  
  555. OutofNode();
  556.  
  557. // else check left
  558.  
  559. bool cr = false;
  560. // bool right = false;
  561. bool cl = false;
  562. //bool left = false;
  563.  
  564. if (!cr)
  565. {
  566. drive.SetDriveRequest request = new drive.SetDriveRequest(0, .05, 100);
  567. _nxtDrivePort.DriveDistance(request);
  568.  
  569. if (isBlack())
  570. {
  571. // right = true;
  572. if (turnRight)
  573. {
  574. Response();
  575. }
  576.  
  577. else
  578. {
  579. drive.SetDriveRequest request2 = new drive.SetDriveRequest(0.05, 0, 200);
  580. _nxtDrivePort.DriveDistance(request2);
  581. Response();
  582. }
  583. }
  584. cr = true;
  585. }
  586.  
  587. else if (!cl)
  588. {
  589.  
  590. drive.SetDriveRequest request = new drive.SetDriveRequest(0.05, 0, 100);
  591. _nxtDrivePort.DriveDistance(request);
  592. if (isBlack())
  593. {
  594. if (!turnRight)
  595. {
  596. Response();
  597. }
  598.  
  599. else
  600. {
  601. drive.SetDriveRequest request2 = new drive.SetDriveRequest(0.0, 0.05, 200);
  602. _nxtDrivePort.DriveDistance(request2);
  603. Response();
  604. }
  605. }
  606.  
  607. cl = true;
  608. }
  609.  
  610.  
  611.  
  612.  
  613. }
  614.  
  615. }
  616. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement