public void InitializeSystem()
{
debug = new OutputPort((Cpu.Pin)PortMap.debug, false);
debug.Write(true);
coder = new Coder();
Thread.Sleep(17000);
coder.Start();
Info("Initializing System...");
navigation = new NavigationManager();
Info("Initialized NavigationManager");
sensors = new SensorManager();
Info("Initialized SensorManager");
battery = new BatteryManager();
Info("Initialized BatteryManager");
currentMode = Mode.SearchingForWall;
sensorTimer = new Timer(new TimerCallback(sensorTimer_tick), new object(), 0, GlobalVal.transmissionPeriodSensor_ms);
positionTimer = new Timer(new TimerCallback(posTimer_Tick), new object(), 0, GlobalVal.transmissionPeriodPosition_ms);
batteryTimer = new Timer(new TimerCallback(batteryTimer_Tick), new object(), 0, GlobalVal.transmissionPeriodBattery_ms);
magTimer = new Timer(new TimerCallback(magnetometer_Tick), new object(), 0, GlobalVal.trasmissionPeriodMagnetometer_ms);
Info("Initialized Timers");
cancel = false;
debug.Write(false);
navigation.manualSpeed = GlobalVal.speed;
navigation.manualTurningSpeed = GlobalVal.turningSpeed;
Info("Ready");
}
public void TurnLeftUntilWall(SensorManager sensors)
{
brake();
//MoveForward(100, GlobalVal.speed);
turnLeft(45);
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect)
{
float wall = sensors.getDistance(Sensor.Wall);
float wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
{
break;
}
else
{
if (wall < wall_back)
{
turnRight(5);
MoveForward(10, GlobalVal.speed);
continue;
}
else
{
turnLeft(10);
MoveForward(10, GlobalVal.speed);
continue;
}
}
}
}
public void MoveToObject(SensorManager sensors)
{
this.resetDistance();
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect && !contingency.alarm)
{
MoveForward(50, GlobalVal.speed);
}
this.brake();
}
public void TurnRightUntilWall(SensorManager sensors)
{
Pid = false;
float central, wall, wall_back;
//double distancia_pared;
K = 10;
if (Pid == true)
{
central = sensors.getDistance(Sensor.Central);
turnRight((int)(central / (central - GlobalVal.minDistanceToFollowWall)));
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
{
break;
}
else
{
central = sensors.getDistance(Sensor.Central);
//distancia_pared = (exMath.cos(Relative_MAG_Heading)) * (central);
//turnRight((int)(distancia_pared / (distancia_pared - GlobalVal.minDistanceToFollowWall)) * K);
}
MoveForward(5);
}
}
if (Pid == false)
{
brake();
int s = 55;
turnRight(s);
Debug.Print("angle_Def:" + s.ToString());
//Thread.Sleep(600);
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
{
break;
}
else
{
turnRight(5);
}
}
}
}
public void TurnRightUntilWall(SensorManager sensors)
{
Pid = false;
float central, wall, wall_back;
//double distancia_pared;
K = 10;
if (Pid == true)
{
central = sensors.getDistance(Sensor.Central);
turnRight((int)(central / (central - GlobalVal.minDistanceToFollowWall)));
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
break;
else
{
central = sensors.getDistance(Sensor.Central);
//distancia_pared = (exMath.cos(Relative_MAG_Heading)) * (central);
//turnRight((int)(distancia_pared / (distancia_pared - GlobalVal.minDistanceToFollowWall)) * K);
}
MoveForward(5);
}
}
if (Pid == false)
{
brake();
int s=55;
turnRight(s);
Debug.Print("angle_Def:" + s.ToString());
//Thread.Sleep(600);
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
break;
else
turnRight(5);
}
}
}
public void TurnLeftUntilWall(SensorManager sensors)
{
brake();
//MoveForward(100, GlobalVal.speed);
turnLeft(45);
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect)
{
float wall = sensors.getDistance(Sensor.Wall);
float wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis) { break; }
else
{
if (wall < wall_back)
{
turnRight(5);
MoveForward(10, GlobalVal.speed);
continue;
}
else
{
turnLeft(10);
MoveForward(10, GlobalVal.speed);
continue;
}
}
}
}
public void MoveToObject(SensorManager sensors)
{
this.resetDistance();
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect && !contingency.alarm) { MoveForward(50, GlobalVal.speed); }
this.brake();
}
public void InitializeSystem()
{
debug = new OutputPort((Cpu.Pin)PortMap.debug, false);
debug.Write(true);
coder = new Coder();
Thread.Sleep(17000);
coder.Start();
Info("Initializing System...");
navigation = new NavigationManager();
Info("Initialized NavigationManager");
sensors = new SensorManager();
Info("Initialized SensorManager");
battery = new BatteryManager();
Info("Initialized BatteryManager");
currentMode = Mode.SearchingForWall;
sensorTimer = new Timer(new TimerCallback(sensorTimer_tick), new object(), 0, GlobalVal.transmissionPeriodSensor_ms);
positionTimer = new Timer(new TimerCallback(posTimer_Tick), new object(), 0, GlobalVal.transmissionPeriodPosition_ms);
batteryTimer = new Timer(new TimerCallback(batteryTimer_Tick), new object(), 0, GlobalVal.transmissionPeriodBattery_ms);
magTimer = new Timer(new TimerCallback(magnetometer_Tick), new object(), 0, GlobalVal.trasmissionPeriodMagnetometer_ms);
Info("Initialized Timers");
cancel = false;
debug.Write(false);
navigation.manualSpeed = GlobalVal.speed;
navigation.manualTurningSpeed = GlobalVal.turningSpeed;
Info("Ready");
}
