Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using System;//
- using System.Collections.Generic;
- using System.ComponentModel;
- using Microsoft.Ccr.Core;
- using Microsoft.Dss.Core.Attributes;
- using Microsoft.Dss.ServiceModel.Dssp;
- using Microsoft.Dss.ServiceModel.DsspServiceBase;
- using W3C.Soap;
- using submgr = Microsoft.Dss.Services.SubscriptionManager;
- using lightsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.LightSensor.Proxy;
- using touchsensor = Microsoft.Robotics.Services.Sample.Lego.Nxt.TouchSensor.Proxy;
- using buttons = Microsoft.Robotics.Services.Sample.Lego.Nxt.Buttons.Proxy;
- using drive = Microsoft.Robotics.Services.Sample.Lego.Nxt.Drive.Proxy;
- using colorv2 = Microsoft.Robotics.Services.Sample.HiTechnic.ColorV2.Proxy;
- using System.Timers;
- namespace Lab4_203_02
- {
- [Contract(Contract.Identifier)]
- [DisplayName("Lab4_203_02")]
- [Description("Lab4_203_02 service (no description provided)")]
- class Lab4_203_02Service : DsspServiceBase
- {
- bool spotlight = false; // flag to determine whether spotlight is on
- int red = 0;
- int green = 0;
- int blue = 0;
- int colournumber = 0;
- bool yellow1 = false;
- bool blue1 = false;
- bool blue2 = false;
- bool green1 = false;
- bool green2 = false;
- bool green3 = false;
- bool green4 = false;
- bool red1 = false;
- bool red2 = false;
- bool red3 = false;
- bool red4 = false;
- bool red5 = false;
- bool back = false;
- public static System.Timers.Timer LeaveNode;
- /// <summary>
- /// Service state
- /// </summary>
- [ServiceState]
- Lab4_203_02State _state = new Lab4_203_02State();
- /// <summary>
- /// Main service port
- /// </summary>
- [ServicePort("/Lab4_203_02", AllowMultipleInstances = true)]
- Lab4_203_02Operations _mainPort = new Lab4_203_02Operations();
- [SubscriptionManagerPartner]
- submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();
- /// <summary>
- /// NxtLightSensor partner
- /// </summary>
- [Partner("NxtLightSensor", Contract = lightsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
- lightsensor.LightSensorOperations _nxtLightSensorPort = new lightsensor.LightSensorOperations();
- /// <summary>
- /// NxtTouchSensor partner
- /// </summary>
- [Partner("NxtTouchSensor", Contract = touchsensor.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
- touchsensor.TouchSensorOperations _nxtTouchSensorPort = new touchsensor.TouchSensorOperations();
- /// <summary>
- /// NxtButtons partner
- /// </summary>
- [Partner("NxtButtons", Contract = buttons.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
- buttons.ButtonOperations _nxtButtonsPort = new buttons.ButtonOperations();
- /// <summary>
- /// NxtDrive partner
- /// </summary>
- [Partner("NxtDrive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
- drive.DriveOperations _nxtDrivePort = new drive.DriveOperations();
- /// <summary>
- /// HiTechnicColorV2Sensor partner
- /// </summary>
- [Partner("HiTechnicColorV2Sensor", Contract = colorv2.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
- colorv2.ColorV2SensorOperations _hiTechnicColorV2SensorPort = new colorv2.ColorV2SensorOperations();
- /// <summary>
- /// Service constructor
- /// </summary>
- public Lab4_203_02Service(DsspServiceCreationPort creationPort)
- : base(creationPort)
- {
- }
- protected override void Start()
- {
- base.Start();
- touchsensor.TouchSensorOperations touchsensorNotificationPort = new touchsensor.TouchSensorOperations();
- _nxtTouchSensorPort.Subscribe(touchsensorNotificationPort);
- Activate(
- Arbiter.Receive<touchsensor.TouchSensorUpdate>
- (true, touchsensorNotificationPort, TouchSensorHandler));
- lightsensor.LightSensorOperations lightsensorNotificationPort = new lightsensor.LightSensorOperations();
- _nxtLightSensorPort.Subscribe(lightsensorNotificationPort);
- Activate(
- Arbiter.Receive<lightsensor.Replace>
- (true, lightsensorNotificationPort, LightSensorHandler));
- buttons.ButtonOperations buttonNotificationPort = new buttons.ButtonOperations();
- _nxtButtonsPort.Subscribe(buttonNotificationPort);
- Activate(
- Arbiter.Receive<buttons.ButtonsUpdate>
- (true, buttonNotificationPort, ButtonPressHandler));
- drive.DriveOperations driverNotificationPort = new drive.DriveOperations();
- _nxtDrivePort.Subscribe(driverNotificationPort);
- Activate(
- Arbiter.Receive<buttons.ButtonsUpdate>
- (true, buttonNotificationPort, ButtonPressHandler));
- colorv2.ColorV2SensorOperations hiTechnicColorV2SensorPort = new colorv2.ColorV2SensorOperations();
- _hiTechnicColorV2SensorPort.Subscribe(hiTechnicColorV2SensorPort);
- Activate(
- Arbiter.Receive<colorv2.ColorV2SensorUpdate>
- (true, hiTechnicColorV2SensorPort, colorHandler));
- }
- [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
- public virtual IEnumerator<ITask> GetHandler(Get get)
- {
- get.ResponsePort.Post(_state);
- yield break;
- }
- /// <summary>
- /// Handle TouchSensor Notifications
- /// </summary>
- /// <param name="notification">Update notification</param>
- ///
- // if touchsensor is pressed check if spotlight is true if so make false if false make true
- private void TouchSensorHandler(touchsensor.TouchSensorUpdate notification)
- {
- if (notification.Body.TouchSensorOn)
- {
- spotlight = !spotlight;
- _nxtLightSensorPort.Spotlight(spotlight);
- //<------- MOVING METHOD CALLS------>
- //MovingLeft();
- //MovingRight30();
- //MovingLeft();
- //MovingSlowly();
- //Turn180();
- //<------- END OF MOVING METHOD CALLS------>
- //<------- COLOUR CHECK CALLS------>
- //isBlack();
- //isWhite();
- //isRed();
- //isGreen();
- //isBlue();
- //isYellow();
- //<------- MOVING METHOD CALLS------>
- }
- }
- public void colorHandler(colorv2.ColorV2SensorUpdate notification)
- {
- red = notification.Body.Red;
- green = notification.Body.Green;
- blue = notification.Body.Blue;
- colournumber = notification.Body.ColorNumber;
- if (isWhite())
- {
- Response();
- }
- else if (isBlack())
- {
- Response();
- }
- else if (isYellow())// now at the only yellow
- {
- if (!yellow1)
- {
- findPaths(true);// check to see if there are any paths
- yellow1 = true; // take the right path
- }
- else
- {
- findPaths(false); // if you have already visited the yellow node then turn left
- }
- }
- else if (isBlue()) // if on blue node
- {
- if (!blue1) // if blue 1 is false
- {
- findPaths(true); // turn right
- blue1 = true; //make blue 1 true
- }
- else if (back && !blue2)
- {
- findPaths(false); // if on way back then turn left (should this not be right)
- }
- else if (!back && blue2)
- {
- findPaths(true);
- }
- else if (back && !blue2)
- {
- findPaths(false);
- }
- }
- else if (isGreen()) // if on blue node
- {
- if (!green1 && !back) // if blue 1 is false
- {
- findPaths(true); // since only one path go straight
- green1 = true; //make blue 1 true
- }
- else if (back && !green2)
- {
- findPaths(true); // take the right path
- }
- else if (!back && !green2)
- {
- findPaths(false);
- green2 = true;
- }
- else if (back && green2)
- {
- findPaths(false);// take the path on the right
- }
- else if (!back && !green3)
- {
- findPaths(true);
- green3 = true; // if on way back then turn left (should this not be right)
- }
- else if (back && green3)
- {
- findPaths(true);
- }
- else if (!back && !green4)
- {
- findPaths(true);
- green4 = true; // if on way back then turn left (should this not be right)
- }
- else if (back && green4)
- {
- findPaths(true);
- }
- }
- else if (isRed()) // if on blue node
- {
- if (!red1 && !back)
- {
- red1 = true;
- Turn180();
- }
- else if (red1 && back)
- {
- findPaths(true);// go straight
- }
- else if (!red2 && !back)
- {
- red2 = true;
- Turn180();
- }
- else if (red2 && back)
- {
- findPaths(true);
- }
- else if (!red3 && !back)
- {
- red3 = true;
- Turn180();
- }
- else if (red3 && back)
- {
- findPaths(true);
- }
- else if (!red4 && !back)
- {
- red4 = true;
- Turn180();
- }
- else if (red4 && back)
- {
- findPaths(true);
- }
- else if (!red5 && !back)
- {
- red5 = true;
- Turn180();
- }
- else if (red5 && back)
- {
- findPaths(true);
- }
- }
- }
- public void Turn180()
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0.5, -0.5, 0);
- _nxtDrivePort.DriveDistance(request);
- Console.WriteLine("in180");
- System.Threading.Thread.Sleep(500);
- }// check this
- private void ButtonPressHandler(buttons.ButtonsUpdate notification)
- {
- if (notification.Body.PressedRight)// when the right button is pressed
- {
- Console.WriteLine("the bot should drive");
- // <--- Checking Line Correction code ----->
- Response();
- }
- else if (notification.Body.PressedLeft)// left button is pressed
- {
- Console.WriteLine("the bot should stop");
- _nxtDrivePort.DriveDistance(0, 0, 0); // set all values to stop the robot from moving
- }
- } // right press drive, left button presss stop
- private void LightSensorHandler(lightsensor.Replace notification)
- {
- }
- public void SubscribeHandler(Subscribe subscribe)
- {
- SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
- } // end of subscribe
- public void Response()
- {
- if (isBlack())
- {
- MovingSlowly();
- }
- if (isWhite())
- {
- System.Timers.Timer whiteTimer = new System.Timers.Timer();
- whiteTimer.Elapsed += new ElapsedEventHandler(checkLeft);
- whiteTimer.Interval = 1000;
- MovingRight30();
- whiteTimer.Enabled = true;
- Console.WriteLine("timer enabled");
- while (isBlack() == false)
- {
- }
- // if (isBlack) {
- whiteTimer.Enabled = false;
- Console.WriteLine("timer disabled");
- MovingSlowly();
- //}
- }
- }
- public void checkLeft(object source, ElapsedEventArgs e)
- {
- Console.WriteLine("Timer elapsed");
- MovingLeft();
- }
- public Boolean isBlack()
- {
- if (colournumber == 13 || colournumber == 14)
- {
- if (red <= 100 && blue <= 100 && green <= 100)
- {
- Console.WriteLine("On Black");
- return true;
- }
- else return false;
- }
- else return false;
- }
- public Boolean isWhite()
- {
- if (colournumber == 13 || colournumber == 14)
- {
- if (red >= 240 && blue >= 240 && green >= 240)
- {
- Console.WriteLine("On White");
- return true;
- }
- else return false;
- }
- else return false;
- }// print line finished
- public Boolean isRed()
- {
- if (colournumber == 10)
- {
- Console.WriteLine(" Is red");
- return true;
- }
- return false;
- }// print line finished
- public Boolean isYellow()
- {
- if (colournumber == 13 || colournumber == 14)
- {
- if (red <= 100 && blue <= 100 && green <= 100)
- {
- Console.WriteLine("Yellow");
- return true;
- }
- else return false;
- }
- else return false;
- }// print line is finished
- public Boolean isGreen()
- {
- if (colournumber == 5)
- {
- Console.WriteLine("Green");
- return true;
- }
- else return false;
- }// print line is finished
- public Boolean isBlue()
- {
- if (colournumber == 3)
- {
- Console.WriteLine("Blue");
- return true;
- }
- else return false;
- }// print line is finished
- public Boolean isFinishGreen()
- {
- if (colournumber == 4)
- {
- Console.WriteLine(" Found EndPoint");
- return true;
- }
- else return false;
- }
- public void MovingSlowly()
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0.2, 0.2, 0);
- _nxtDrivePort.DriveDistance(request);
- Console.WriteLine(" Moving slowly");
- }// (0.2, 0.2, 0)
- public void MovingLeft()// (0, 0.3, 35)
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0, 0.3, 35);
- _nxtDrivePort.DriveDistance(request);
- System.Threading.Thread.Sleep(500);
- Console.WriteLine("left");
- }
- public void MovingRight30() // (0, .03, 60);// line following
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0, .03, 60);
- _nxtDrivePort.DriveDistance(request);
- System.Threading.Thread.Sleep(500);
- Console.WriteLine(" Moving right 30");
- }
- public void StopMoving()
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0, 0, 0);
- _nxtDrivePort.DriveDistance(request);
- System.Threading.Thread.Sleep(500);
- Console.WriteLine("Stopped Moving");
- }// (0,0,0)
- public void OutofNode() //moving out of node Should we enable a timer?
- {
- LeaveNode = new System.Timers.Timer();
- LeaveNode.Elapsed += new ElapsedEventHandler(EndOfLeavingNode);
- LeaveNode.Interval = 1500; // milliseconds
- LeaveNode.Enabled = true;
- drive.SetDriveRequest request = new drive.SetDriveRequest(0.03, 0.03, 0);
- Response();
- Console.WriteLine("Out of Node");
- }
- public void EndOfLeavingNode( object o , ElapsedEventArgs e)
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0.00, 0.00, 00);
- }
- 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.
- {
- OutofNode();
- // else check left
- bool cr = false;
- // bool right = false;
- bool cl = false;
- //bool left = false;
- if (!cr)
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0, .05, 100);
- _nxtDrivePort.DriveDistance(request);
- if (isBlack())
- {
- // right = true;
- if (turnRight)
- {
- Response();
- }
- else
- {
- drive.SetDriveRequest request2 = new drive.SetDriveRequest(0.05, 0, 200);
- _nxtDrivePort.DriveDistance(request2);
- Response();
- }
- }
- cr = true;
- }
- else if (!cl)
- {
- drive.SetDriveRequest request = new drive.SetDriveRequest(0.05, 0, 100);
- _nxtDrivePort.DriveDistance(request);
- if (isBlack())
- {
- if (!turnRight)
- {
- Response();
- }
- else
- {
- drive.SetDriveRequest request2 = new drive.SetDriveRequest(0.0, 0.05, 200);
- _nxtDrivePort.DriveDistance(request2);
- Response();
- }
- }
- cl = true;
- }
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement