/// <summary>
/// Occures when the application encounters an unhandled exception. The applicatoin is unloaded and all resources released
/// </summary>
/// <param name="sender">The source that caused the exception</param>
/// <param name="e">The exception object entry</param>
private void Application_UnhandledException(object sender, Windows.UI.Xaml.UnhandledExceptionEventArgs e)
{
RobotController.SetRobotStatus(RobotStatus.Exception);
NetworkUtils.LogNetwork(e.ToString(), MessageType.Exception);
Current.Exit();
}
/// <summary>
/// Reauests a poweroff of the system
/// </summary>
/// <param name="delay">The dealy before the poweroff takes place</param>
public static void Poweroff(TimeSpan delay)
{
NetworkUtils.LogNetwork(string.Format("Shutting down in {0} seconds", delay.TotalSeconds), MessageType.Warning);
ShutdownManager.BeginShutdown(ShutdownKind.Shutdown, delay);
}
/// <summary>
/// Requests a reboot of the system
/// </summary>
/// <param name="delay">The delay before the reboot takes place</param>
public static void Reboot(TimeSpan delay)
{
NetworkUtils.LogNetwork(string.Format("Rebooting in {0} seconds", delay.TotalSeconds), MessageType.Warning);
ShutdownManager.BeginShutdown(ShutdownKind.Restart, delay);
}
/// <summary>
/// Default contorl method for when the robot is in regular cleaning mode
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private static void ControlRobotAuto(object sender, DoWorkEventArgs e)
{
//https://social.msdn.microsoft.com/Forums/vstudio/en-US/42e694a0-843a-4f7f-81bc-69e1ae662e9f/how-to-lower-the-thread-priority?forum=csharpgeneral
if (System.Threading.Thread.CurrentThread.Priority != System.Threading.ThreadPriority.Highest)
{
System.Threading.Thread.CurrentThread.Priority = System.Threading.ThreadPriority.Highest;
}
if (ControlStatus != ControlStatus.Auto)
{
ControlStatus = ControlStatus.Auto;
}
RobotAutoControlState = AutoControlState.None;
Hardware.ResetI2CData(true, true);
Hardware.RightEncoder.ResetCounter();
Hardware.LeftEncoder.ResetCounter();
Manual_counter_1 = Manual_counter_2 = left_ir_count_reset = right_ir_count_reset = 0;
while (true)
{
//check for cancel/abort
if (ControllerThread.CancellationPending)
{
e.Cancel = true;
return;
}
//update sensor values
//battery
Hardware.UpdateSignalBattery();
SignalBatteryStatus = Hardware.UpdateSignalBatteryStatus();
Hardware.Signal_battery_status_indicator.UpdateRuntimeValue((int)SignalBatteryStatus);
Hardware.UpdatePowerBattery();
PowerBatteryStatus = Hardware.UpdatePowerBatteryStatus();
Hardware.Power_battery_status_indicator.UpdateRuntimeValue((int)PowerBatteryStatus);
//GPIO
Hardware.UpdateGPIOValues();
//I2C
if (DelayI2CRead >= 2)
{
Hardware.UpdateI2CData(0, 1);
DelayI2CRead = -1;
}
DelayI2CRead++;
//SPI
Hardware.UpdateSPIData();
//IR
if (left_ir_count_reset++ > SIDE_IR_CLEAR_BUFFER_TIMEOUT)
{
Hardware.SideReciever.ClearDetectionBuffer();
left_ir_count_reset = 0;
}
if (right_ir_count_reset++ > FRONT_IR_CLEAR_BUFFER_TIMOEUT)
{
Hardware.FrontReciever.ClearDetectionBuffer();
right_ir_count_reset = 0;
}
//process battery conditions
if (!IGNORE_LOW_SIGNAL_BATTERY_ACTION)
{
switch (SignalBatteryStatus)
{
case BatteryStatus.Below5Shutdown:
if (LastSignalBattStatus != SignalBatteryStatus)
{
LastSignalBattStatus = SignalBatteryStatus;
NetworkUtils.LogNetwork("Robot signal voltage has reached critical level and must shut down, Setting shutdown for 60s", MessageType.Error);
EmergencyShutdown(TimeSpan.FromSeconds(60));
}
break;
case BatteryStatus.Below15Warning:
if (LastSignalBattStatus != SignalBatteryStatus)
{
LastSignalBattStatus = SignalBatteryStatus;
NetworkUtils.LogNetwork("Robot signal voltage has reached warning level, needs to go back to base", MessageType.Warning);
}
break;
}
}
if (!IGNORE_LOW_POWER_BATTERY_ACTION)
{
if (LastPowerBattStatus != PowerBatteryStatus)
{
LastPowerBattStatus = PowerBatteryStatus;
switch (PowerBatteryStatus)
{
case BatteryStatus.Below5Shutdown:
NetworkUtils.LogNetwork("Robot power voltage has reached critical level, needs to go back to base", MessageType.Warning);
break;
case BatteryStatus.Below15Warning:
NetworkUtils.LogNetwork("Robot power voltage has reached warning level, needs to go back to base", MessageType.Warning);
break;
}
}
}
//check water level
//Hardware.WaterLevel > 1.5F && !ReachedWaterLimit
if (false)//ignore water leverl for now
{
NetworkUtils.LogNetwork("Robot water level is at level, needs to dump", MessageType.Warning);
RobotAutoControlState = AutoControlState.OnWaterLimit;
ReachedWaterLimit = true;//will be set to false later
}
switch (RobotAutoControlState)
{
case AutoControlState.None:
//getting into here means that the robot has not started, has good batteries, and is not water level full
Hardware.SideReciever.Start();
Hardware.FrontReciever.Start();
RobotAutoControlState = AutoControlState.TurnToMap;
if (WorkArea == null)
{
WorkArea = new Map();
}
NumSidesMapped = 0;
NetworkUtils.LogNetwork("On robot init, autocontrol state none->TurnToMap", MessageType.Info);
SingleSetBool = false;
break;
case AutoControlState.TurnToMap:
//turn to right to get to first laser reading
//move encoders specific ammount
if (Hardware.SideReciever.WallDetected) //it makes it to the wall
{
if (SingleSetBool)
{
NetworkUtils.LogNetwork("Robot has found side wall, TurnToMap->MapSideBackup", MessageType.Info);
Hardware.LeftEncoder.ResetCounter();
Hardware.RightEncoder.ResetCounter();
Hardware.RightDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(ALL_STOP);
RobotAutoControlState = AutoControlState.MapSideBackup;
SingleSetBool = false;
}
}
else if (!SingleSetBool)
{
Hardware.RightDrive.SetActiveDutyCyclePercentage(SLOW_REVERSE);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(SLOW_LEFT_FORWARD_MAP);
SingleSetBool = true;
}
break;
case AutoControlState.MapOneSide:
//if front IR sensor
// save counter rotations
// move to turn to map phase
//if wall IR sensor
// turn x rotations
//else if not wall ir sensor and counter for turning
// set back to slow right turn
if (Hardware.FrontReciever.WallDetected)
{
NetworkUtils.LogNetwork("Front wall detected, MapOneSide->MapOverTurn", MessageType.Info);
RobotAutoControlState = AutoControlState.MapOverTurn;
SingleSetBool = false;
}
else if (Hardware.SideReciever.WallDetected)
{
NetworkUtils.LogNetwork("Side IR detected wall, need to turn small left, MapOneSide->MapSideBackup", MessageType.Info);
Hardware.LeftEncoder.ResetCounter();
RobotAutoControlState = AutoControlState.MapSideBackup;
SingleSetBool = false;
}
else if (!SingleSetBool)
{
Hardware.LeftDrive.SetActiveDutyCyclePercentage(SLOW_LEFT_FORWARD_MAP);
Hardware.RightDrive.SetActiveDutyCyclePercentage(SLOW_RIGHT_FORWARD_MAP);
SingleSetBool = true;
}
break;
case AutoControlState.MapSideBackup:
if (!SingleSetBool)
{
Hardware.RightDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(SLOW_REVERSE);
SingleSetBool = true;
}
else if (Hardware.LeftEncoder.Clicks <= SIDE_WALL_CORRECTION)
{
//go back to a slow turn to the right
NetworkUtils.LogNetwork("Left encoder has backed up enough, moving to slow right movement, MapSideBackup->MapOneSide", MessageType.Info);
Hardware.LeftEncoder.ResetCounter();
Hardware.SideReciever.ResetDetection();
Hardware.LeftDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.RightDrive.SetActiveDutyCyclePercentage(ALL_STOP);
RobotAutoControlState = AutoControlState.MapOneSide;
SingleSetBool = false;
}
break;
case AutoControlState.MapOverTurn:
if (!SingleSetBool)
{
//save the rightEncoder value
right_encoder_side_value = Hardware.RightEncoder.Clicks;
Hardware.RightEncoder.ResetCounter();
//convert it to a normalized distance (TODO)
float MPU_height = Hardware.PositionX;
//convert it to a normalized distance
//average it
Hardware.LeftEncoder.ResetCounter();
Hardware.RightDrive.SetActiveDutyCyclePercentage(SLOW_FORWARD);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(0.6F);
SingleSetBool = true;
}
else if (Hardware.LeftEncoder.Clicks >= MAP_TURN_OVER_BOUNDRY)
{
NetworkUtils.LogNetwork("Left encoder has moved enough over boundry, MapOverTurn->MapTurn", MessageType.Info);
RobotAutoControlState = AutoControlState.MapTurn;
SingleSetBool = false;
}
break;
case AutoControlState.MapTurn:
if (!SingleSetBool)
{
Hardware.LeftEncoder.ResetCounter();
Hardware.RightDrive.SetActiveDutyCyclePercentage(SLOW_FORWARD);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(SLOW_REVERSE);
switch (NumSidesMapped)
{
case 0:
NetworkUtils.LogNetwork(string.Format("Side {0} mapped, using width encoder value of {1}, not averaging", NumSidesMapped, right_encoder_side_value), MessageType.Info);
WorkArea.SetHeight(right_encoder_side_value, false);
break;
case 1:
NetworkUtils.LogNetwork(string.Format("Side {0} mapped, using height encoder value of {1}, not averaging", NumSidesMapped, right_encoder_side_value), MessageType.Info);
WorkArea.SetWidth(right_encoder_side_value, false);
break;
case 2:
NetworkUtils.LogNetwork(string.Format("Side {0} mapped, using second width encoder value of {1}, averaging", NumSidesMapped, right_encoder_side_value), MessageType.Info);
WorkArea.SetHeight(right_encoder_side_value, true);
break;
case 3:
NetworkUtils.LogNetwork(string.Format("Side {0} mapped, using second height encoder value of {1}, averaging", NumSidesMapped, right_encoder_side_value), MessageType.Info);
WorkArea.SetWidth(right_encoder_side_value, true);
break;
default:
NetworkUtils.LogNetwork(string.Format("Sides value of {0} should not happen", NumSidesMapped), MessageType.Error);
break;
}
NumSidesMapped++;
if (NumSidesMapped > 3)
{
NetworkUtils.LogNetwork("Mapping finished, moving to calculations, MapTurn->Calculations", MessageType.Info);
RobotAutoControlState = AutoControlState.Calculations;
SingleSetBool = false;
}
else
{
SingleSetBool = true;
}
}
else if (Hardware.LeftEncoder.Clicks <= MAP_TURN_LEFT)
{
NetworkUtils.LogNetwork("Left encoder has backed up enough for conter, moving to slow right movement on new wall, MapTurn->MapOneSide", MessageType.Info);
//reset the side reciever to let it continue
Hardware.SideReciever.ResetDetection();
Hardware.FrontReciever.ResetDetection();
//DEBUG:TEMP SET BACK TO MAKE A SQUARE
RobotAutoControlState = AutoControlState.MapOneSide;
SingleSetBool = false;
}
break;
case AutoControlState.Calculations:
if (!SingleSetBool)
{
Hardware.RightDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(ALL_STOP);
//set robot location
WorkArea.SetRobotLocation(0F, 0F);
NetworkUtils.LogNetwork("Sending mapping data...", MessageType.Info);
NetworkUtils.LogNetwork(WorkArea.XMLMap, MessageType.Mapping);
RobotAutoControlState = AutoControlState.TurnToClean;
NetworkUtils.LogNetwork("Finish calculations, Calculcations->TurnToClean", MessageType.Info);
SingleSetBool = false;
}
break;
case AutoControlState.TurnToClean:
if (!SingleSetBool)
{
Hardware.RightEncoder.ResetCounter();
Hardware.LeftEncoder.ResetCounter();
Hardware.ResetI2CData(true, true);
Hardware.RightDrive.SetActiveDutyCyclePercentage(0.63F);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(SLOW_REVERSE);
//you would set the cleaning relays on here
SingleSetBool = true;
}
if (Hardware.LeftEncoder.Clicks <= TURN_TO_CLEAN || Hardware.RotationX >= 800)
{
NetworkUtils.LogNetwork(string.Format("right encoder clicks={0}, rotationX={1}, TurnToClean->CleanUp", Hardware.RightEncoder.Clicks, Hardware.RotationX), MessageType.Info);
RobotAutoControlState = AutoControlState.CleanUp;
SingleSetBool = false;
}
break;
case AutoControlState.CleanUp:
if (!SingleSetBool)
{
Hardware.RightDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.LeftDrive.SetActiveDutyCyclePercentage(ALL_STOP);
SingleSetBool = true;
}
break;
case AutoControlState.CleanTurn:
break;
case AutoControlState.CleanDown:
break;
case AutoControlState.CleanComplete:
break;
case AutoControlState.OnLowPowerBattery:
break;
case AutoControlState.OnLowSignalBattery:
break;
case AutoControlState.OnWaterLimit:
break;
case AutoControlState.OnObstructionWhenCleaning:
//don't have the hardware to do this...
break;
case AutoControlState.OnObstuctionWhenMapping:
//don't have the hardware to do this...
break;
}
System.Threading.Thread.Sleep(TimeSpan.FromMilliseconds(5));
}
}
/// <summary>
/// The method for the control thread to run when manual debugging control is requested
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private static void ControlRobotManual(object sender, DoWorkEventArgs e)
{
//https://social.msdn.microsoft.com/Forums/vstudio/en-US/42e694a0-843a-4f7f-81bc-69e1ae662e9f/how-to-lower-the-thread-priority?forum=csharpgeneral
if (System.Threading.Thread.CurrentThread.Priority != System.Threading.ThreadPriority.Highest)
{
System.Threading.Thread.CurrentThread.Priority = System.Threading.ThreadPriority.Highest;
}
if (ControlStatus != ControlStatus.Manual)
{
ControlStatus = ControlStatus.Manual;
}
NetworkUtils.LogNetwork("Manual control method starting", MessageType.Debug);
if (!Hardware.SideReciever.Enabled)
{
NetworkUtils.LogNetwork("Starting side reciever", MessageType.Debug);
Hardware.SideReciever.Start();
}
if (!Hardware.FrontReciever.Enabled)
{
NetworkUtils.LogNetwork("Starting front reciever", MessageType.Debug);
Hardware.FrontReciever.Start();
}
Manual_counter_1 = Manual_counter_2 = left_ir_count_reset = right_ir_count_reset = 0;
Hardware.ResetI2CData(true, true);
while (true)
{
if (ControllerThread.CancellationPending)
{
e.Cancel = true;
return;
}
//if there is no network connection
if (!NetworkUtils.ConnectionLive)
{
//stop moving
Hardware.LeftDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.RightDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.Auger_pin.Write(GpioPinValue.High);
Hardware.Impeller_pin.Write(GpioPinValue.High);
System.Threading.Thread.Sleep(20);
continue;
}
//update sensor values
//battery
Hardware.UpdateSignalBattery();
SignalBatteryStatus = Hardware.UpdateSignalBatteryStatus();
Hardware.Signal_battery_status_indicator.UpdateRuntimeValue((int)SignalBatteryStatus);
Hardware.UpdatePowerBattery();
PowerBatteryStatus = Hardware.UpdatePowerBatteryStatus();
Hardware.Power_battery_status_indicator.UpdateRuntimeValue((int)PowerBatteryStatus);
//GPIO
Hardware.UpdateGPIOValues();
//I2C
Hardware.UpdateI2CData(0, 1);
//SPI
Hardware.UpdateSPIData();
//IR
if (left_ir_count_reset++ > FRONT_IR_CLEAR_BUFFER_TIMOEUT)
{
Hardware.SideReciever.ClearDetectionBuffer();
left_ir_count_reset = 0;
}
if (right_ir_count_reset++ > FRONT_IR_CLEAR_BUFFER_TIMOEUT)
{
Hardware.FrontReciever.ClearDetectionBuffer();
right_ir_count_reset = 0;
}
if (Hardware.SideReciever.WallDetected && Manual_counter_1++ > 40)
{
Hardware.SideReciever.ResetDetection();
Manual_counter_1 = 0;
}
if (Hardware.FrontReciever.WallDetected && Manual_counter_2++ > 40)
{
Hardware.FrontReciever.ResetDetection();
Manual_counter_2 = 0;
}
//parse the write the commands
string[] commands = NetworkUtils.ManualControlCommands.Split(',');
//left (float), right (float), motor (bool)
try
{
Hardware.LeftDrive.SetActiveDutyCyclePercentage(float.Parse(commands[0]));
Hardware.RightDrive.SetActiveDutyCyclePercentage(float.Parse(commands[1]));
Hardware.Auger_pin.Write(bool.Parse(commands[2]) ? GpioPinValue.Low : GpioPinValue.High);
Hardware.Impeller_pin.Write(bool.Parse(commands[2]) ? GpioPinValue.Low : GpioPinValue.High);
}
catch
{
Hardware.LeftDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.RightDrive.SetActiveDutyCyclePercentage(ALL_STOP);
Hardware.Auger_pin.Write(GpioPinValue.High);
Hardware.Impeller_pin.Write(GpioPinValue.High);
}
System.Threading.Thread.Sleep(25);
}
}
/// <summary>
/// Initializes the I2C controller and set default values of devices attached to I2C
/// </summary>
/// <returns>true if successfull init, false otherwise</returns>
public static async Task <bool> InitI2C()
{
//https://github.com/Microsoft/Windows-universal-samples/blob/master/Samples/IoT-I2C/cs/Scenario1_ReadData.xaml.cs
I2C_Controller = await I2cController.GetDefaultAsync();
if (I2C_Controller == null)
{
return(false);
}
I2C_Connection_settings = new I2cConnectionSettings(ADDRESS)//MPU-6050 address
{
BusSpeed = I2cBusSpeed.FastMode,
};
MPU6050 = I2C_Controller.GetDevice(I2C_Connection_settings);
if (MPU6050 == null)
{
return(false);
}
await Task.Delay(3); // wait power up sequence
//device powers up in sleep mode, need to take it out of sleep mode
//0x80 sets the device to reset so we have a working state to use
try
{
I2C_WriteByte(PWR_MGMT_1, 0x80);
NetworkUtils.LogNetwork("MPU connection sucessfull, verified with reset bit send", MessageType.Debug);
}
catch
{
return(false);
}
//allow for the reset to occur
await Task.Delay(100);
//test to make sure the device is on the correct bus
byte address_test = I2C_ReadByte(WHO_AM_I);
NetworkUtils.LogNetwork(string.Format("MPU reports that device is on address {0}...", address_test), MessageType.Debug);
if (!address_test.Equals(ADDRESS))
{
return(false);
}
//"Upon power up, the MPU-60X0 clock source defaults to the internal oscillator. However, it is highly
//recommended that the device be configured to use one of the gyroscopes(or an external clock
//source) as the clock reference for improved stability"
//using x axis gyroscope
I2C_WriteByte(PWR_MGMT_1, 0x1);
NetworkUtils.LogNetwork("MPU internal clock set to use X axis gyroscope", MessageType.Debug);
//reset and disable the FIFO (don't need it)
NetworkUtils.LogNetwork("MPU FIFO reset and disable and signal cond clear reset", MessageType.Debug);
//disable all sensors from writing to the FIFO
I2C_WriteByte(FIFO_EN, 0x00);
//reset and complely disable FIFO, and reset signal conditons while clearing signal registers. nice.
I2C_WriteByte(USER_CTRL, 0x05);
NetworkUtils.LogNetwork("Config gyro and accel sensitivity", MessageType.Debug);
//config gyro to be +/- 250 degrees/sec
I2C_WriteByte(GYRO_CONFIG, 0);
//config accel to be +/- 2g
I2C_WriteByte(ACCEL_CONFIG, 0);
NetworkUtils.LogNetwork("Config gyro and accel sample rate and filter rate", MessageType.Debug);
//config DLPF to be 10/10 HZ, 13.8/13.4 ms delay (secondmost maximum filtering)
//TODO: determine if this is too much filtering?
//https://www.youtube.com/watch?v=Bv5ajMgdsno
I2C_WriteByte(CONFIG, 0x06);
//use a 50Hz sample rate
I2C_WriteByte(SMPLRT_DIV, 19);
NetworkUtils.LogNetwork("Disable inturrupts and clear accel and gyro values", MessageType.Debug);
I2C_WriteByte(INT_ENABLE, 0x00);
return(true);
}