public bool Open()
{
bool result = false;
try
{
Plc = CreateModbusClient(Ip, Port, ReceiveTimeout);
if (Plc != null)
{
result = true;
Status = DriverStatus.Connected;
}
else
{
result = false;
Status = DriverStatus.Disconnected;
}
}
catch (Exception ex)
{
logger.Error($"ex={ex}");
Status = DriverStatus.Disconnected;
}
logger.Debug($"[{Status}] [{result}] Ip={Ip}, Port={Port}, ReceiveTimeout={ReceiveTimeout}");
return(result);
}
public void start()
{
lock (syncLock)
{
LogUtil.logInfo("config:" + config.ToString());
if (!config.isValid())
{
throw new Exception("Config for the driver is incomplete");
}
try
{
if (config.waitTime != null)
{
_waitTime = Convert.ToInt32(config.waitTime);
}
}
catch (Exception e)
{
LogUtil.logInfo("Error using the waitTime of " + config.waitTime + ". Defaulting to " + DEFAULT_WAIT_TIME);
_waitTime = DEFAULT_WAIT_TIME;
}
initInput();
initConfirm();
initOk();
if (!pendingStop)
{
status = DriverStatus.Running;
}
}
}
private void SubmitNextTask(IActiveContext activeContext)
{
Logger.Log(Level.Info, "SubmitNextTask with evaluator id: " + activeContext.EvaluatorId);
IConfiguration finalConfiguration = GetNextTaskConfiguration();
if (null != finalConfiguration)
{
Logger.Log(Level.Info, "Executing task id " + _taskContext.CurrentTaskId());
Logger.Log(Level.Info, string.Format(CultureInfo.InvariantCulture, "Submitting Task {0}", _taskContext.CurrentTaskId()));
activeContext.SubmitTask(finalConfiguration);
}
else
{
if (_taskContext.TaskCompleted == _taskContext.TotalTasks)
{
Logger.Log(Level.Info, "All tasks submitted and completed, active context remains idle");
_driveStatus = DriverStatus.Idle;
if (!_isRetain)
{
activeContext.Dispose();
}
}
}
}
GetStatusDictionaryCopy()
{
statusesLock.EnterReadLock();
try {
var copy = new Dictionary <DriverModuleName, DriverStatus>();
foreach (DriverModuleName dmn in this.driversStatus.Keys)
{
DriverStatus source = this.driversStatus[dmn];
var destStatus = new DriverStatus {
IsActivated = source.IsActivated,
IsDeactivationPending = source.IsDeactivationPending,
IsActivationPending = source.IsActivationPending
};
copy[dmn] = destStatus;
}
return(copy);
}
finally {
statusesLock.ExitReadLock();
}
}
public void WaitForDriverToFinish()
{
DriverStatus status = FetchFirstDriverStatus();
if (DriverStatus.UNKNOWN == status)
{
// We were unable to connect to the Driver at least once.
throw new WebException("Unable to connect to the Driver.");
}
while (status.IsActive())
{
// Add sleep in while loop, whose value alligns with default heart beat interval.
Thread.Sleep(DriverStatusIntervalInMilliSecond);
LOGGER.Log(Level.Info, "DriverStatus is {0}", status);
try
{
status = FetchDriverStatus();
}
catch (WebException e)
{
// If we no longer can reach the Driver, it must have exited.
status = DriverStatus.UNKNOWN_EXITED;
LOGGER.Log(Level.Warning, "Driver unreacheable. Exiting now.", e);
}
}
}
public void OnHttpRequest(ReefHttpRequest request, ReefHttpResponse response)
{
string target = ParsePathInfo(request.PathInfo);
LOGGER.Log(Level.Info, "Target: " + target + ". PathInfo: " + request.PathInfo);
//if (target != null && target.ToLower(CultureInfo.CurrentCulture).Equals("driverstatus"))
if (target != null && target.Equals("driverstatus"))
{
LOGGER.Log(Level.Info, "Target: " + target + ". Driver status: " + driveStatus.ToString());
string msg = string.Format(CultureInfo.CurrentCulture, "Current Driver status: {0} ", driveStatus.ToString());
BuildHttpResponse(response, HttpStatusCode.OK, msg);
return;
}
if (target != null && target.Equals("taskstatus"))
{
LOGGER.Log(Level.Info, "Target: " + target + ". TaskStatus string: " + _taskContext.TaskStatusString());
BuildHttpResponse(response, HttpStatusCode.OK, _taskContext.TaskStatusString());
return;
}
if (target != null && target.ToLower(CultureInfo.CurrentCulture).Equals("run") && driveStatus == DriverStatus.Init)
{
BuildHttpResponse(response, HttpStatusCode.OK, "Driver is not ready, wait a few second then send request again!!!");
return;
}
if (target != null && target.ToLower(CultureInfo.CurrentCulture).Equals("run") && driveStatus == DriverStatus.RunningTasks)
{
string msg = string.Format(CultureInfo.CurrentCulture,
"A job is running. Please check driver status and then submit your job again.");
BuildHttpResponse(response, HttpStatusCode.OK, msg);
return;
}
if (target != null && target.ToLower(CultureInfo.CurrentCulture).Equals("run") && driveStatus == DriverStatus.Idle)
{
string numberOfTasks = getQueryValue(request.Querystring, "numberoftasks");
if (numberOfTasks == null)
{
BuildHttpResponse(response, HttpStatusCode.OK, "Please specify number of tasks to run");
return;
}
driveStatus = DriverStatus.RunningTasks;
using (LOGGER.LogFunction("HelloSimpleEventHandlers::Processing a new Job from web request"))
{
_taskContext = new TaskContext();
_taskContext.TotalTasks = int.Parse(numberOfTasks, CultureInfo.CurrentCulture);
BuildHttpResponse(response, HttpStatusCode.OK, "Job from web request is submitted and is running!!!");
}
foreach (var c in _activeContexts)
{
SubmitNextTask(c);
}
return;
}
BuildHttpResponse(response, HttpStatusCode.OK, "Unsupported query");
}
public Driver(string cNH, string cNHCategory, int?vehicleId, Vehicle vehicle, DriverStatus status, int id, string name, string cPF, DateTime birthDate, string phone, string email, string cEP, string street, int number, string neighborhood, string city, UF uF) : base(id, name, cPF, birthDate, phone, email, cEP, street, number, neighborhood, city, uF)
{
CNH = cNH;
CNHCategory = cNHCategory;
VehicleId = vehicleId;
Vehicle = vehicle;
Status = status;
}
public ActionResult DeleteConfirmed(int id)
{
DriverStatus driverStatus = db.DriversStatuses.Find(id);
db.DriversStatuses.Remove(driverStatus);
db.SaveChanges();
return(RedirectToAction("Index"));
}
public async Task <bool> UpdateStatus(string driverId, DriverStatus status)
{
var update = Builders <Driver> .Update.Set(t => t.Status, status);
var result = await this.collection.UpdateOneAsync(t => t.DriverId == driverId, update);
return(result.ModifiedCount == 1);
}
/// <summary>
/// Modbus 코일(1bit bool) 영역, 레지스터(16bit ushort, Word) 영역, 레지스터 2개 더블워드(32bit int) 데이터를 읽기.
/// 다른영역을 한번에 읽으면 예외발생.
/// 더블워드(32bit int)는 바이트오더(ABCD, CDAB)를 고려하여 파싱하여야 한다.
/// </summary>
/// <param name="slaveId"></param>
/// <param name="startAddress"></param>
/// <param name="count"></param>
/// <returns></returns>
public async Task <(int error, ushort[] results)> Read(
string stringAddress, ushort startAddress = 1, int count = 1, bool isBit = false,
ushort slaveId = 1, int registerType = 1)
{
int error = 0;
ushort[] results = null;
if (Plc == null)
{
return(-1, results);
}
//Modbus Function Code. Bit(CoilStatus = 0, InputStatus=1) Word(InputRegister = 2, HoldingRegister =3)
var type = (FunctionCode)Enum.Parse(typeof(FunctionCode), $"{registerType}");
try
{
switch (type)
{
case FunctionCode.Coil:
var boolResults = await Plc.ReadCoilsAsync((byte)slaveId, startAddress, (ushort)count);
results = boolResults.Select(m => Convert.ToUInt16(m)).ToArray();
break;
case FunctionCode.Input:
var boolResults2 = await Plc.ReadInputsAsync((byte)slaveId, startAddress, (ushort)count);
results = boolResults2.Select(m => Convert.ToUInt16(m)).ToArray();
break;
case FunctionCode.InputRegister:
results = await Plc.ReadInputRegistersAsync((byte)slaveId, startAddress, (ushort)count);
//var results2 = ModbusMaster.ReadHoldingRegisters32((byte)slaveAddress, startAddress, count);
break;
case FunctionCode.HoldingRegister:
results = await Plc.ReadHoldingRegistersAsync((byte)slaveId, startAddress, (ushort)count);
break;
default:
logger.Error($"Invalid registerType={registerType}");
break;
}
}
catch (Exception ex)
{
logger.Error($"ex.HResult={ex.HResult}, slaveId={slaveId}, startAddress={startAddress}, count={count}, ex={ex}");
error = ex.HResult;
Status = DriverStatus.Error;
}
return(error, results);
}
public HelloSimpleEventHandlers()
{
LOGGER.Log(Level.Info, "HelloSimpleEventHandlers constructor");
CreateClassHierarchy();
Identifier = "HelloSimpleEventHandlers";
_taskContext = new TaskContext();
_taskContext.TotalTasks = NumberOfTasks;
driveStatus = DriverStatus.Init;
}
public void OnNext(IActiveContext activeContext)
{
using (Logger.LogFunction("HelloSimpleEventHandlers::activeContext received"))
{
Logger.Log(Level.Info, string.Format(CultureInfo.InvariantCulture, "Received activeContext, EvaluatorId id: {0}", activeContext.EvaluatorId));
_activeContexts.Add(activeContext);
_driveStatus = DriverStatus.RunningTasks;
SubmitNextTask(activeContext);
}
}
public void stop()
{
if (status == DriverStatus.Running || status == DriverStatus.Error)
{
pendingStop = true;
status = DriverStatus.Shuttingdown;
Thread threadStop = new Thread(stopThread);
threadStop.Start();
}
}
static public int ChangeDriverStatus(Int64 driverID, DriverStatus newStatus, Int64?tripID = null)
{
// STable table = new STable(SharedLibDefines.TableLogisticsDrivers);
// ELogisticsDriver eDriver = table.SelectByID<ELogisticsDriver>(driverID);
// if (string.IsNullOrEmpty(eDriver.mobile)) eDriver.mobile = null; //senao o mysql reclama: Duplicate entry '' for key 'mobile_UNIQUE'
// eDriver.driverStatus = (int)newStatus;
// //STripLogs.Insert($"Alterando o status do motorista {eDriver.name} para " + newStatus.ToString(), tripID, null, driverID);
// if (SLogisticsDriversLogs.IsLogEnabled(eDriver)) SLogisticsDriversLogs.Insert(new ELogisticsDriverLog { driverID = eDriver.id, content = "Alterando o status do motorista para ", driverStatus = eDriver.driverStatus });
// table.Save(eDriver);
// return (int)newStatus;
return(0);
}
private HelloSimpleEventHandlers(IEvaluatorRequestor evaluatorRequestor,
[Parameter(typeof(NumberOfEvaluators))] int numberOfEvaluators,
[Parameter(typeof(IsRetain))] bool isRetain)
{
Logger.Log(Level.Info, "HelloSimpleEventHandlers constructor");
_taskContext = new TaskContext();
_taskContext.TotalTasks = NumberOfTasks;
_driveStatus = DriverStatus.Init;
_isRetain = isRetain;
_numberOfEvaluators = numberOfEvaluators;
_evaluatorRequestor = evaluatorRequestor;
}
private async Task AssertExcepted(Driver driver, DriverStatus expectedStatus)
{
var expected = new Driver
{
DriverId = driver.DriverId,
Name = driver.Name,
Status = expectedStatus
};
var actual = await this.fixture.GetDriverFromDb(driver.DriverId);
DeepAssert.Equal(expected, actual, "Id");
}
public ActionResult Generate(GatePassVM gatePass)
{
ApplicationUser user = System.Web.HttpContext.Current.GetOwinContext().GetUserManager <ApplicationUserManager>().FindById(System.Web.HttpContext.Current.User.Identity.GetUserId());
if (ModelState.IsValid)
{
var vehicleId = db.Vehicles.Where(e => e.RegistrationNo == gatePass.RegistrationNo).Select(e => e.Id).FirstOrDefault();
DriverStatus dv = new DriverStatus();
dv.UserId = user.Id;
dv.AssignedRequestId = gatePass.AssignedRequestId;
dv.ConfigurationId = gatePass.ConfigurationId;
dv.OnTime = DateTime.Now;
VehicleStatus vs = new VehicleStatus();
vs.VehicleId = vehicleId;
vs.AssignedRequestId = gatePass.AssignedRequestId;
vs.ConfigurationId = gatePass.ConfigurationId;
vs.OnTime = DateTime.Now;
vs.UserId = user.Id;
if (gatePass.GatePassBy)
{
var driverId = db.Employees.Where(e => e.UserId == dv.UserId).Select(e => e.Id).FirstOrDefault();
dv.EmployeeId = driverId;
vs.EmployeeId = dv.EmployeeId;
}
else
{
var driverId = db.Employees.Where(e => e.EmpIdNo == gatePass.EmpIdNo && e.IsDriver == true).Select(e => e.Id).FirstOrDefault();
dv.EmployeeId = driverId;
vs.EmployeeId = dv.EmployeeId;
}
db.DriversStatuses.Add(dv);
// db.SaveChanges();
db.VehiclesStatuses.Add(vs);
db.SaveChanges();
return(RedirectToAction("Index", "Home"));
}
//ViewBag.AssignedRequestId = new SelectList(db.AssignedRequests, "Id", "UserId", driverStatus.AssignedRequestId);
// ViewBag.EmployeeId = new SelectList(db.Employees, "Id", "Name", driverStatus.EmployeeId);
var status =
db.Configurations.Include(e => e.ConfigurationType)
.Where(e => e.ConfigurationType.Type == "DriverStatus");
ViewBag.ConfigurationId = new SelectList(status, "Id", "Name");
return(View());
}
public ActionResult Edit([Bind(Include = "Id,UserId,EmployeeId,AssignedRequestId,ConfigurationId,OnTime,IsDeleted")] DriverStatus driverStatus)
{
if (ModelState.IsValid)
{
db.Entry(driverStatus).State = EntityState.Modified;
db.SaveChanges();
return(RedirectToAction("Index"));
}
ViewBag.AssignedRequestId = new SelectList(db.AssignedRequests, "Id", "UserId", driverStatus.AssignedRequestId);
ViewBag.ConfigurationId = new SelectList(db.Configurations, "Id", "Name", driverStatus.ConfigurationId);
ViewBag.EmployeeId = new SelectList(db.Employees, "Id", "Name", driverStatus.EmployeeId);
return(View(driverStatus));
}
// GET: DriverStatus/Details/5
public ActionResult Details(int?id)
{
if (id == null)
{
return(new HttpStatusCodeResult(HttpStatusCode.BadRequest));
}
DriverStatus driverStatus = db.DriversStatuses.Find(id);
if (driverStatus == null)
{
return(HttpNotFound());
}
return(View(driverStatus));
}
public void SetDriverStatus(Driver driver, DriverStatus status)
{
driver.SetStatus(status);
var routeEntries = GetRouteEntries(driver.Id);
OrderService.StartRoute(routeEntries);
if (status == DriverStatus.Driving)
{
driver.CurrentRoute.SetStartTime();
}
DriverRepository.Update(driver);
PersistenceContext.SaveChanges();
}
private void SetState(DriverStatus value)
{
Log.Verbose("{State} --> {Value}", _state, value);
lock (_stateSync)
{
_state = value;
}
if (_state == DriverStatus.HARDWARE_READY)
{
PostHardwareEnabled();
}
}
public void SetStatus(DriverModuleName n, DriverStatus s)
{
this.statusesLock.EnterWriteLock();
try {
if (this.driversStatus.ContainsKey(n) == false)
{
throw new InvalidDriverModuleName(n.ToString());
}
this.driversStatus[n] = s;
}
finally {
this.statusesLock.ExitWriteLock();
}
}
public NetworkManager(ICommandInterface commandInterface, NetworkMode mode, long timeout)
{
_announceListenerFilter.OnStateChanged += (object sender, DriverStatus status) => SetState(status);
_afMessageListenerFilter = new AFMessageListenerFilter(_messageListeners);
_mode = mode;
_commandInterface = commandInterface;
Timeout = DEFAULT_TIMEOUT;
ResetTimeout = RESET_TIMEOUT_DEFAULT;
StartupTimeout = STARTUP_TIMEOUT_DEFAULT;
ResendOnlyException = RESEND_ONLY_EXCEPTION_DEFAULT;
_state = DriverStatus.CLOSED;
}
public string Startup()
{
// Called when the network is first started
if (_state != DriverStatus.CLOSED)
{
throw new InvalidOperationException("Driver already opened, current status is:" + _state);
}
_state = DriverStatus.CREATED;
_logger.Trace("Initializing hardware.");
// Open the hardware port
SetState(DriverStatus.HARDWARE_INITIALIZING);
if (!InitializeHardware())
{
Shutdown();
return(null);
}
// Now reset the dongle
SetState(DriverStatus.HARDWARE_OPEN);
if (!DongleReset())
{
_logger.Warn("Dongle reset failed. Assuming bootloader is running and sending magic byte {MagicByte}.", "0x" + BOOTLOADER_MAGIC_BYTE.ToString("X2"));
if (!bootloaderGetOut(BOOTLOADER_MAGIC_BYTE))
{
_logger.Warn("Attempt to get out from bootloader failed.");
Shutdown();
return(null);
}
}
SetState(DriverStatus.HARDWARE_READY);
string version = GetStackVersion();
if (version == null)
{
_logger.Debug("Failed to get CC2531 version");
}
else
{
_logger.Info("CC2531 version is {Version}", version);
}
return(version);
}
public async Task <IHttpActionResult> RegisterDriverStatus(DriverStatus model)
{
try
{
if (!ModelState.IsValid)
{
var modelErrors = new List <string>();
foreach (var modelState in ModelState.Values)
{
foreach (var modelError in modelState.Errors)
{
modelErrors.Add(modelError.ErrorMessage);
}
}
return(Content(HttpStatusCode.BadRequest, MessageResponse.Message(HttpStatusCode.BadRequest.ToString(), modelErrors[0].ToString()), new JsonMediaTypeFormatter()));
}
var userKey = "DriverStatus_" + model.VehicleID;
if (await _bucket.ExistsAsync(userKey))
{
//return Content(HttpStatusCode.Conflict, new Error($"Driver Status '{model.VehicleID}' already exists"));
return(Content(HttpStatusCode.Conflict, MessageResponse.Message(HttpStatusCode.Conflict.ToString(), "166-Vehicle ID already exists"), new JsonMediaTypeFormatter()));
}
// call third part api to check Vehicle is valid or not
var driverStatusMessageDoc = new Document <DriverStatus>()
{
Id = userKey,
Content = new DriverStatus
{
DriverID = userKey,
DriverState = model.DriverState,
Date = model.Date,
Time = model.Time,
VehicleID = model.VehicleID,
}
};
var result = await _bucket.InsertAsync(driverStatusMessageDoc);
if (!result.Success)
{
return(Content(HttpStatusCode.InternalServerError, MessageResponse.Message(HttpStatusCode.InternalServerError.ToString(), result.Message), new JsonMediaTypeFormatter()));
}
return(Content(HttpStatusCode.OK, MessageResponse.Message(HttpStatusCode.OK.ToString(), MessageDescriptions.Add, result.Document.Id), new JsonMediaTypeFormatter()));
}
catch (Exception ex)
{
return(Content(HttpStatusCode.InternalServerError, MessageResponse.Message(HttpStatusCode.InternalServerError.ToString(), ex.StackTrace), new JsonMediaTypeFormatter()));
}
}
// GET: DriverStatus/Edit/5
public ActionResult Edit(int?id)
{
if (id == null)
{
return(new HttpStatusCodeResult(HttpStatusCode.BadRequest));
}
DriverStatus driverStatus = db.DriversStatuses.Find(id);
if (driverStatus == null)
{
return(HttpNotFound());
}
ViewBag.AssignedRequestId = new SelectList(db.AssignedRequests, "Id", "UserId", driverStatus.AssignedRequestId);
ViewBag.ConfigurationId = new SelectList(db.Configurations, "Id", "Name", driverStatus.ConfigurationId);
ViewBag.EmployeeId = new SelectList(db.Employees, "Id", "Name", driverStatus.EmployeeId);
return(View(driverStatus));
}
private void Profile_DriverStatusReceived(object sender, DriverStatus e)
{
string oldValue = _lastDriverStatus;
if (!DriverDescriptions.TryGetValue(e.DriverType, out _lastDriverStatus))
{
// just use the raw value
_lastDriverStatus = e.DriverType;
}
ConfigManager.LogManager.LogDebug($"received profile status indicating that simulator is running exports for '{e.DriverType}'");
if (oldValue != _lastDriverStatus)
{
UpdateStatusMessage();
// send only a simple status message to status viewer instead of whole status via ReportStatusToStatusViewer
ReportStatus($"Simulator is running {_lastDriverStatus}");
}
}
public bool Close()
{
bool result = false;
try
{
Plc.Close();
result = true;
Status = DriverStatus.Disconnected;
}
catch (Exception ex)
{
logger.Error($"ex={ex}");
}
logger.Debug($"[{Status}] [{result}] LogicalStationNumber={LogicalStationNumber}");
return(result);
}
public void stopThread()
{
for (int counter = 0; counter < 10; counter++)
{
lock (syncLock)
{
if (currentMessagesToBeProcessed <= 0 && !isInputThreadRunning && !isthreadOkOfacRunning && !isthreadConfirmOfacRunning)
{
status = DriverStatus.NotRunning;
return;
}
}
Thread.Sleep(5000);
}
status = DriverStatus.Error;
}
public void EnableDriver(DriverModuleName moduleName)
{
if (moduleName == null)
{
throw new InvalidDriverModuleName("null");
}
if (this.driversSource.GetStatus(moduleName).IsActivated)
{
return;
}
if (this.driverPropsGetter.RequiresRestartToWork(moduleName))
{
HashSet <DriverModuleName> driversWaitingActivation
= this.pendingChanges.Activation;
if (driversWaitingActivation.Contains(moduleName))
{
return;
}
this.pendingChanges.OnEnableRequest(moduleName);
var s = new DriverStatus {
IsActivated = false,
IsActivationPending = true,
IsDeactivationPending = false
};
this.driversSource.SetStatus(moduleName, s);
}
else
{
var s = new DriverStatus {
IsActivated = true,
IsActivationPending = false,
IsDeactivationPending = false
};
this.driversSource.SetStatus(moduleName, s);
}
}
private static Message CreateStatusMessageIfChanged(LiveDriver driver, DriverStatus status)
{
return driver.Status != status ? new SetDriverStatusMessage(driver.Id, status) : null;
}
public void can_parse_a_driver_status(GridColumnColour colour, DriverStatus expected)
{
Assert.Equal(expected, LiveData.ToDriverStatus(colour));
}
/// <summary>
/// Tries to change the status of this driver.
/// </summary>
/// <param name="newStatus">The new driver status.</param>
public void ChangeStatus(DriverStatus newStatus)
{
if(Status == newStatus)
{
return;
}
switch(newStatus)
{
case DriverStatus.InPits:
++LapNumber;
CurrentSectorNumber = 1;
break;
}
Status = newStatus;
}
/// <summary>
/// Initialises a new instance of the <see cref="SetDriverStatusMessage"/> class and
/// specifies if the Id of the driver and the driver's status.
/// </summary>
/// <param name="driverId">The Id of the driver.</param>
/// <param name="driverStatus">The driver's new status.</param>
/// <exception cref="System.ArgumentOutOfRangeException">
/// Thrown when <paramref name="driverId"/> is not positive.
/// </exception>
public SetDriverStatusMessage(int driverId, DriverStatus driverStatus)
: base(driverId)
{
DriverStatus = driverStatus;
}
public void CommandEmergencyStop()
{
SetEngineVelocity(0, 0);
lock (commandQueue)
{
commandQueue.Clear();
}
status = DriverStatus.Done;
}
public void Process()
{
//read position and direction
updateDirection();
rangeFinder.UpdateValue();
Vector curnetPosition = new Vector(Position[0], Position[1]);
//TODO: map update
//TODO: colision detection
Command toDo;
lock (commandQueue)
{
if (commandQueue.Count == 0)
{ //nothing to do
if (status == DriverStatus.Processing)
status = DriverStatus.Done;
return;
}
toDo = commandQueue[0];
}
status = DriverStatus.Processing;
Vector goal = new Vector(toDo.x, toDo.y);
Vector dV;
switch (toDo.type)
{
case Command.CType.LookAt:
goal.Sub(curnetPosition);
goal.Normalize();
dV = new Vector(goal);
dV.Sub(direction);
if (dV.Lenght() < rotationEpsilon || goal.Lenght() == 0.0)
{
SetEngineVelocity(0, 0);
lock (commandQueue)
commandQueue.RemoveAt(0);
return;
}
bool rotateRight = direction.IsRotateRightAGoodIdea(goal);
// set the engine velocity
double vel = maxVelocity * RotationSpeedMap(dV.Lenght());
if (rotateRight)
{
SetEngineVelocity(vel, -vel);
}
else
{
SetEngineVelocity(-vel, vel);
}
break;
case Command.CType.GoTo:
// obstacle detection
if (rangeFinder.value < obstacleDetectionRange)
{
CommandEmergencyStop();
status = DriverStatus.Obstacle;
return;
}
dV = new Vector(goal);
dV.Sub(curnetPosition);
if (dV.Lenght() < positionEpsilon)
{
SetEngineVelocity(0, 0);
lock (commandQueue)
commandQueue.RemoveAt(0);
return;
}
double leftVel, rightVel;
leftVel = rightVel = maxVelocity * MovementSpeedMap(dV.Lenght());
//correction direction
Vector dDirection = new Vector(dV);
dDirection.Normalize();
Vector dDv = new Vector(dDirection);
dDv.Sub(direction);
if (dDv.Lenght() > rotationEpsilon)
{
if (direction.IsRotateRightAGoodIdea(dDirection))
{
rightVel -= rightVel * dDv.Lenght(); //rotationEpsilon < dDv.Length <= 2.0, 2.0 mean 180 degree error
}
else
{
leftVel -= leftVel * dDv.Lenght();
}
}
SetEngineVelocity(leftVel, rightVel);
break;
}
}
public void SetRobot(Robot r)
{
this.robot = r;
status = DriverStatus.Done;
rangeFinder = robot.GetSensorByName(rangeFinderName) as Robot.SensorRangeFinder;
leftWhell = robot.GetPartByName(leftWheelName);
rightWheel = robot.GetPartByName(rightWheelName);
direction = new Vector(0, 0);
updateDirection();
commandQueue = new List<Command>();
}
