Hi
Im working on a distance / collision detection for a little bot…
Im starting it as a thread, and it fires a event when collision is detected.
Problem is that when it fires the event. the thread dont continue to run.
So im guessing im doing this threading thing wrong
Anyone who can help?
using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using GHIElectronics.NETMF.FEZ;
using Adafruit_16channel;
using PingAndRotate;
using Parallax;
namespace Adafruit_16PWM {
public class Program {
public static Thread CollisionDetect = new Thread(StartCollisionDetect);
public static void Main() {
CollisionDetect.Start();
}
private static CollisionDetect _collisionDetect = null;
private static void StartCollisionDetect() {
Adafruit_PWMServoDriver servo = new Adafruit_PWMServoDriver();
Ping ping = new Ping((Cpu.Pin)FEZ_Pin.Digital.Di34);
_collisionDetect = new CollisionDetect(servo,ping,0);
_collisionDetect.CollisionEvent += new CollisionDetect.CollisionEventDelegate(CollisionDetectedEvent);
_collisionDetect.Start();
}
static void CollisionDetectedEvent(object sender, CollisionDetect.CollisionEventArgs e) {
Debug.Print("Collision at: " + e.ServoPos);
Debug.Print("Distance: " + e.PingDistance);
Debug.Print("Getting new route...");
Debug.Print("New route at ServoPos: "+ _collisionDetect.GetBestRoute().ToString());
//Now the thread stops... and dont detect distance anymore,
}
}
}
using System;
using Microsoft.SPOT;
using Adafruit_16channel;
using Parallax;
using System.Threading;
using System.Collections;
namespace PingAndRotate {
class CollisionDetect {
private ushort ServoMin = 0;
private ushort ServoCenter = 0;
private ushort ServoMax = 0;
private ushort ServoCurrentPos = 0;
private int StoppingDistance = 0;
private Adafruit_PWMServoDriver Servo = null;
private byte ServoNum = 0;
private Ping Pinger = null;
private Thread PingThread = null;
private bool _readerRunning = false;
public CollisionDetect(Adafruit_PWMServoDriver _Servo, Ping _Pinger, byte _ServoNum = 0, int _StoppingDistance = 20, ushort _ServoMin = 170, ushort _ServoMax = 550, ushort _ServoCenter = 350) {
Servo = _Servo;
ServoNum = _ServoNum;
Pinger = _Pinger;
ServoMin = _ServoMin;
ServoCenter = _ServoCenter;
ServoMax = _ServoMax;
StoppingDistance = _StoppingDistance;
}
public void Start() {
// Do nothing if already running.
if (_readerRunning) return;
ServoMove(ServoCenter);
PingThread = new Thread(CheckDistance);
PingThread.Start();
_readerRunning = true;
}
public void Stop() {
// Do nothing if not running.
if (!_readerRunning) return;
ServoMove(ServoCenter);
PingThread.Abort();
_readerRunning = false;
}
public void CheckDistance() {
bool isStopped = false;
int distance = GetDistance();
while (true) {
if (distance <= StoppingDistance) {
if (!isStopped) {
Debug.Print("Stopping distance reached... Maybe look for new route?");
CollisionEventArgs _args = new CollisionEventArgs();
_args.ServoPos = ServoCurrentPos;
// _args.BestRoute = GetBestRoute();
_args.PingDistance = distance;
_args.Stop = true;
OnCollisionEvent(_args);
isStopped = true;
}
} else {
distance = GetDistance();
if (isStopped) {
Debug.Print("All is good, move forward");
CollisionEventArgs _args = new CollisionEventArgs();
_args.ServoPos = ServoCurrentPos;
_args.PingDistance = distance;
_args.Stop = false;
OnCollisionEvent(_args);
isStopped = false;
}
}
Thread.Sleep(500);
}
}
private int GetDistance() {
int distance = Pinger.GetDistance(Ping.DistanceUnits.Centimeters);
//Debug.Print("Distance in centimeters: " + distance.ToString());
return distance;
}
public ushort GetBestRoute() {
ArrayList routes = new ArrayList();
ArrayList RoutesToCheck = new ArrayList();
RoutesToCheck.Add(ServoCenter);
RoutesToCheck.Add(ServoMin);
RoutesToCheck.Add(ServoMax);
ushort bestRoute = 0;
foreach (ushort MoveTo in RoutesToCheck) {
ServoMove(MoveTo);
Thread.Sleep(1000);
Routes route = new Routes();
route.Distance = GetDistance();
route.ServoPos = ServoCurrentPos;
routes.Add(route);
}
Routes temp;
Routes bestRouteTemp = new Routes();
foreach (Routes s in routes) {
temp = s;
if (bestRouteTemp.Distance < temp.Distance) { bestRouteTemp = temp; }
}
bestRoute = bestRouteTemp.ServoPos;
ServoMove(ServoCenter);
return bestRoute;
}
private class Routes {
public ushort ServoPos { get; set; }
public int Distance { get; set; }
}
public void ServoMove(ushort PulseLength) {
if (PulseLength > ServoMax) {
PulseLength = ServoMax;
}
if (PulseLength < ServoMin) {
PulseLength = ServoMin;
}
ServoCurrentPos = PulseLength;
Servo.setPWM(ServoNum, 0, PulseLength);
}
#region Public Event
public delegate void CollisionEventDelegate(object sender, CollisionEventArgs e);
public event CollisionEventDelegate CollisionEvent;
public class CollisionEventArgs : EventArgs {
private ushort _ServoPos;
private ushort _BestRoute;
private int _PingDistance;
private bool _Stop;
public ushort ServoPos {
get { return _ServoPos; }
set { _ServoPos = value; }
}
public ushort BestRoute {
get { return _BestRoute; }
set { _BestRoute = value; }
}
public int PingDistance {
get { return _PingDistance; }
set { _PingDistance = value; }
}
public bool Stop {
get { return _Stop; }
set { _Stop = value; }
}
}
private void OnCollisionEvent(CollisionEventArgs e) {
if (CollisionEvent != null) {
CollisionEvent(this, e);
}
}
#endregion
}
}