public byte[] GetDriveFaultB(Logger_class log = null)
{
if (!_CheckInit(log, true))
{
return(null);
}
byte[] tempReturn;
if (log != null)
{
log.WriteLog("Getting drive fault B status...");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_DRIVE_B_FAULT });
tempReturn = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (log != null)
{
log.WriteLog("Raw Response: " + _BytesToString(tempReturn));
}
}
return(tempReturn);
}
public void SetFailsafe(bool setting, Logger_class log = null)
{
if (!_CheckInit())
{
return;
}
bool currentState = this.GetFailsafe(log);
if (currentState == setting)
{
if (log != null)
{
log.WriteLog("Called setting for failsafe: " + setting.ToString() + " but it already was...");
}
return;
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_SET_FAILSAFE, Convert.ToByte(!currentState) });
if (log != null)
{
log.WriteLog("Set failsafe to " + (!currentState).ToString());
}
}
}
public byte[] GetLED2(Logger_class log)
{
byte[] tempReturn;
if (!_CheckInit())
{
return(null);
}
log.WriteLog("Calling results for get LED 2...");
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_LED2 });
tempReturn = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (tempReturn[0] == COMMAND_GET_LED2)
{
if (log != null)
{
log.WriteLog("Get LED2 response: " + this._BytesToString(tempReturn));
}
return(tempReturn);
}
else
{
if (log != null)
{
log.WriteLog("Got a nonsense reponse from COMMAND_GET_LED2..." + this._BytesToString(tempReturn));
}
throw new InvalidProgramException("Nonsense response during COMMAND_GET_LED2.");
}
}
}
public int GetMotorB(Logger_class log = null)
{
int tempReturn = 0;
if (!_CheckInit(log, true))
{
return(0);
}
if (log != null)
{
log.WriteLog("Getting power level for B motors...");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_B });
byte[] response = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (response == null)
{
if (log != null)
{
log.WriteLog("*** ERROR: no response from ThunderBorg...");
}
throw new NullReferenceException("No parseable response from B motors on GetMotorB request.");
}
else if (response[0] != COMMAND_GET_B)
{
if (log != null)
{
log.WriteLog("Didn't get an expected response from the B motors on GetMotorB request.");
}
throw new IndexOutOfRangeException("Unexpected response from B motors on GetMotorB request.");
}
else
{
byte power_direction = response[1];
byte power_level = response[2];
if (log != null)
{
log.WriteLog("Raw response: " + _BytesToString(response));
}
if (power_direction == COMMAND_VALUE_FWD)
{
tempReturn = power_level;
}
else if (power_direction == COMMAND_VALUE_REV)
{
tempReturn = -power_level;
}
}
}
return(tempReturn);
}
public bool GetFailsafe(Logger_class log = null)
{
if (!_CheckInit())
{
throw new NullReferenceException("ThunderBorg not initiated.");
}
bool tempReturn = false;
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_FAILSAFE });
byte[] response = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (log != null)
{
log.WriteLog("Got response on failsafe: " + _BytesToString(response));
}
if (response[0] == COMMAND_GET_FAILSAFE)
{
tempReturn = Convert.ToBoolean(response[1]);
}
}
return(tempReturn);
}
private bool _CheckInit(Logger_class log = null, bool throwException = false)
{
bool tempReturn = false;
if (this._TBorgAddress == 0x00)
{
if (log != null)
{
log.WriteLog("ThunderBorg not initiated...");
}
if (throwException)
{
throw new ApplicationException("ThunderBorg not initialized.");
}
else
{
tempReturn = false;
}
}
else
{
tempReturn = true;
}
return(tempReturn);
}
public byte GetBoardID(Logger_class log = null)
{
byte tempReturn = 0x00;
if (!_CheckInit())
{
throw new NullReferenceException("ThunderBorg not initiated.");
}
if (log != null)
{
log.WriteLog("Checking board ID...");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_ID });
byte[] response = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (response[0] == COMMAND_GET_ID)
{
tempReturn = response[1];
}
}
return(tempReturn);
}
/// <summary>
/// Sets the battery monitoring limits used for setting the LED color. The colors shown range from full red at minimum (or below), yellow at half, and green at maximum or higher. These values are stored in the EEPROM and reloaded when the board is powered.
/// </summary>
/// <param name="minimum">Minimum voltage (0 V minimum)</param>
/// <param name="maximum">Maximum voltage (36.3 V maximum)</param>
/// <param name="log">Optional logging output routine</param>
public void SetBatteryMonitoringLimits(decimal minimum, decimal maximum, Logger_class log = null)
{
// my original values were 6.98 / 35.02; I don't know what the defaults are
if (!_CheckInit())
{
throw new NullReferenceException("ThunderBorg not initiated.");
}
minimum /= Convert.ToDecimal(VOLTAGE_PIN_MAX);
maximum /= Convert.ToDecimal(VOLTAGE_PIN_MAX);
byte levelMin = Math.Max(Convert.ToByte(0x00), Math.Min(Convert.ToByte(0xFF), Convert.ToByte(minimum * 0xFF)));
byte levelMax = Math.Max(Convert.ToByte(0x00), Math.Min(Convert.ToByte(0xFF), Convert.ToByte(maximum * 0xFF)));
if (log != null)
{
log.WriteLog("Trying to set battery monitoring limits to: 0x" + levelMin.ToString("X2") + " V. min and 0x" + levelMax.ToString("X2") + " V. max...");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_SET_BATT_LIMITS, levelMin, levelMax });
System.Threading.Thread.Sleep(200); // note: this was recommended in the Python version for EEPROM writing
}
}
public ThunderBorg(Logger_class log = null, bool tryOtherBus = false)
{
if (log != null)
{
this._log = log;
log.WriteLog("Finding ThunderBorg...");
_TBorgAddress = ThunderBorg.ScanForThunderBorg(1, log);
}
else
{
_TBorgAddress = ThunderBorg.ScanForThunderBorg();
}
if (log != null)
{
log.WriteLog("Loding ThunderBorg on bus " + _bus.ToString("X2") + ", address " + _TBorgAddress.ToString("X2"));
}
}
public void SetAllMotors(int power, Logger_class log = null)
{
if (!_CheckInit())
{
return;
}
if ((power > 255) || (power < -255))
{
if (log != null)
{
log.WriteLog("Power level out of range -255 <= power <= 255; rejecting command...");
}
throw new IndexOutOfRangeException("Invalid power setting to motors; range outside of -255 <= power <= 255.");
}
if (log != null)
{
log.WriteLog("Setting all motors to: " + power.ToString("X2"));
}
byte parsedPower = 0;
if (power > 0)
{
parsedPower = Convert.ToByte(power);
}
else
{
parsedPower = Convert.ToByte(-power);
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
if (power > 0)
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_SET_ALL_FWD, parsedPower });
}
else
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_SET_ALL_REV, parsedPower });
}
}
}
public static int ScanForThunderBorg(int busNumber = 1, Logger_class log = null)
{
int tempReturn = -1;
if (log != null)
{
log.WriteLog("Starting scan for ThunderBorg board...");
}
using (var bus = RPi.I2C.Net.I2CBus.Open("/dev/i2c-" + busNumber.ToString()))
{
for (byte port = 0x03; port < 0x78; port++)
{
try
{
bus.WriteByte(port, COMMAND_GET_ID);
byte[] response = bus.ReadBytes(port, I2C_MAX_LEN);
if (response[0] == 0x99)
{
if (response[1] == I2C_ID_THUNDERBORG)
{
tempReturn = port;
if (log != null)
{
log.WriteLog("FOUND ThunderBorg board on port: " + port.ToString("X2"));
}
}
}
}
catch (Exception ex)
{
// do nothing
}
}
}
if (log != null)
{
log.WriteLog("Finished port scan...");
}
return(tempReturn);
}
private static void TestLogger(Logger_class log)
{
ILogger.Priority existing = log.DefaultLogLevel;
log.DefaultLogLevel = ILogger.Priority.Medium;
log.WriteLog("Testing overall log capability. I'll write five messages, one for each log level, with");
log.WriteLog("deescalating priorities.");
log.WriteLog();
log.WriteLog("------------------------------------------------------------------");
log.WriteLog("Critical", ILogger.Priority.Critical);
log.WriteLog("High", ILogger.Priority.High);
log.WriteLog("Medium", ILogger.Priority.Medium);
log.WriteLog("Low", ILogger.Priority.Low);
log.WriteLog("Information", ILogger.Priority.Information);
log.DefaultLogLevel = existing;
}
public decimal GetBatteryVoltage(Logger_class log = null)
{
int tempIntReturn = 0;
decimal tempReturn = 0.00M;
if (!_CheckInit())
{
throw new NullReferenceException("ThunderBorg not initiated.");
}
if (log != null)
{
log.WriteLog("Getting battery voltage...");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_BATT_VOLT });
byte[] response = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (log != null)
{
log.WriteLog("GET_BATT_VOLT response: " + this._BytesToString(response));
}
if (response[0] == COMMAND_GET_BATT_VOLT)
{
tempIntReturn = ((int)response[1] << 8) + (int)response[2];
}
}
tempReturn = Convert.ToDecimal(tempIntReturn) / Convert.ToDecimal(COMMAND_ANALOG_MAX);
tempReturn *= VOLTAGE_PIN_MAX;
tempReturn += VOLTAGE_PIN_CORRECTION;
tempReturn = Math.Round(tempReturn, 2);
return(tempReturn);
}
public void AllStop(Logger_class log = null)
{
if (!_CheckInit(log, true))
{
return;
}
if (log != null)
{
log.WriteLog("Calling all stop.");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_ALL_OFF });
}
}
public void SetLED2(byte red, byte green, byte blue, Logger_class log = null)
{
if (!_CheckInit())
{
return;
}
if (log != null)
{
log.WriteLog("Setting LED 2...");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_SET_LED2, red, green, blue });
}
}
public void SetLEDBatteryMonitor(bool setting, Logger_class log = null)
{
if (!_CheckInit())
{
return;
}
if (log != null)
{
log.WriteLog("Setting LED battery monitor to: " + setting.ToString());
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_SET_LED_BATT_MON, Convert.ToByte(setting) });
}
}
public decimal[] GetBatteryMonitoringLimits(Logger_class log = null)
{
decimal tempMin = 0.00M;
decimal tempMax = 0.00M;
if (!_CheckInit())
{
throw new NullReferenceException("ThunderBorg not initiated.");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_BATT_LIMITS });
byte[] response = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (response[0] == COMMAND_GET_BATT_LIMITS)
{
tempMin = response[1];
tempMax = response[2];
tempMin /= Convert.ToDecimal(0xFF);
tempMax /= Convert.ToDecimal(0xFF);
tempMin *= VOLTAGE_PIN_MAX;
tempMax *= VOLTAGE_PIN_MAX;
tempMin = Math.Round(tempMin, 2);
tempMax = Math.Round(tempMax, 2);
decimal[] tempReturn = { tempMin, tempMax };
if (log != null)
{
log.WriteLog("Voltages found - min: " + tempMin.ToString() + " max: " + tempMax.ToString());
}
return(tempReturn);
}
}
return(null);
}
static void Main(string[] args)
{
Console.WriteLine("Hello World!");
Logger_class myLog = new Logger_class();
ThunderBorg_class myBorg = new ThunderBorg_class(myLog);
ThunderBorgSettings_class initialSettings = new ThunderBorgSettings_class();
initialSettings.GetCurrentEnvironment(myBorg, myLog);
TestLogger(myLog);
myLog.DefaultLogLevel = ILogger.Priority.Information;
int intCurrentAddress = ThunderBorg_class.ScanForThunderBorg(log: myLog);
byte currentAddress = Convert.ToByte(intCurrentAddress);
ThunderBorg_class.SetNewAddress(33, currentAddress, 1, myLog);
int newIntCurrentAddress = ThunderBorg_class.ScanForThunderBorg(log: myLog);
byte newCurrentAddress = Convert.ToByte(newIntCurrentAddress);
ThunderBorg_class.SetNewAddress(currentAddress, newCurrentAddress, 1, myLog);
myLog.DefaultLogLevel = ILogger.Priority.Medium;
myBorg.SetFailsafe(false, myLog);
//TestBorg(myBorg, myLog);
//TestLEDs(myBorg, myLog);
//myBorg.SetLEDBatteryMonitor(true, myLog);
//myBorg.WaveLEDs(myLog);
//myBorg.TestSpeeds(myLog);
//myBorg.SetLEDBatteryMonitor(false, myLog);
myLog.WriteLog("Place Borg in location with 1 meter clearance in front, then press any key.");
Console.ReadKey(true);
myBorg.TestDistance(0.5M, myLog);
initialSettings.SetCurrentEnvironment(myBorg, myLog);
}
public bool GetLEDBatteryMonitor(Logger_class log = null)
{
bool tempReturn = false;
if (!_CheckInit())
{
throw new NullReferenceException("ThunderBorg not initiated.");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_GET_LED_BATT_MON });
byte[] response = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
if (log != null)
{
log.WriteLog("Response from COMMAND_GET_LED_BATT_MON: " + this._BytesToString(response));
}
if (response[0] == COMMAND_GET_LED_BATT_MON)
{
if (response[1] == COMMAND_VALUE_ON)
{
tempReturn = true;
return(tempReturn);
}
else if (response[1] == COMMAND_VALUE_OFF)
{
tempReturn = false;
return(tempReturn);
}
}
else
{
throw new InvalidProgramException("Nonsense response during COMMAND_GET_LED_BATT_MON.");
}
}
return(tempReturn);
}
public void SetLEDs(byte red, byte green, byte blue, Logger_class log = null)
{
if (!_CheckInit())
{
return;
}
if (log != null)
{
log.WriteLog("Setting LEDs to " + red.ToString() + ", " + green.ToString() + ", " + blue.ToString() + "...");
}
using (var bus = I2CBus.Open("/dev/i2c-" + this._bus.ToString()))
{
bus.WriteBytes(_TBorgAddress, new byte[] { COMMAND_SET_LEDS, red, green, blue });
byte[] response = bus.ReadBytes(_TBorgAddress, I2C_MAX_LEN);
//if (log != null)
//{
// log.WriteLog("Raw response to SET_LEDS: " + this._BytesToString(response));
//}
}
}
private static void TestBorg(ThunderBorg_class myBorg, Logger_class log)
{
myBorg.SetLEDBatteryMonitor(false, log);
myBorg.SetMotorA(128, log);
System.Threading.Thread.Sleep(1000);
int getx = myBorg.GetMotorA(log);
log.WriteLog("Reporting speed A: " + getx.ToString());
myBorg.GetDriveFaultA(log);
myBorg.AllStop(log);
myBorg.SetMotorB(128, log);
System.Threading.Thread.Sleep(1000);
int gety = myBorg.GetMotorB(log);
log.WriteLog("Reporting speed B: " + gety.ToString());
myBorg.GetDriveFaultB(log);
myBorg.AllStop(log);
myBorg.SetMotorA(-128, log);
System.Threading.Thread.Sleep(1000);
getx = myBorg.GetMotorA(log);
log.WriteLog("Reporting speed A: " + getx.ToString());
myBorg.GetDriveFaultA(log);
myBorg.AllStop(log);
myBorg.SetMotorB(-128, log);
System.Threading.Thread.Sleep(1000);
gety = myBorg.GetMotorB(log);
log.WriteLog("Reporting speed B: " + gety.ToString());
myBorg.GetDriveFaultB(log);
myBorg.AllStop(log);
myBorg.SetAllMotors(128, log);
System.Threading.Thread.Sleep(1000);
myBorg.AllStop();
myBorg.SetAllMotors(-128, log);
System.Threading.Thread.Sleep(1000);
myBorg.AllStop();
myBorg.GetFailsafe(log);
myBorg.SetFailsafe(true, log);
myBorg.SetFailsafe(true, log);
myBorg.SetFailsafe(false, log);
myBorg.SetFailsafe(false, log);
myBorg.GetLED1(log);
myBorg.GetLED2(log);
myBorg.GetLEDBatteryMonitor(log);
myBorg.SetLEDBatteryMonitor(false, log);
myBorg.GetLEDBatteryMonitor(log);
myBorg.SetLED1(255, 255, 255, log);
myBorg.SetLED2(255, 255, 255, log);
System.Threading.Thread.Sleep(1000);
myBorg.SetLEDs(0, 0, 0, log);
log.WriteLog("Battery voltage: " + myBorg.GetBatteryVoltage(log).ToString());
myBorg.SetLEDBatteryMonitor(true, log);
log.WriteLog("Board ID: 0x" + myBorg.GetBoardID(log).ToString("X2"));
myBorg.GetBatteryMonitoringLimits(log);
myBorg.SetBatteryMonitoringLimits(7.0M, 35.0M, log);
}
private static void TestBorg(ThunderBorg myBorg, Logger_class log)
{
myBorg.SetMotorA(128, log);
System.Threading.Thread.Sleep(1000);
int getx = myBorg.GetMotorA(log);
log.WriteLog("Reporting speed A: " + getx.ToString());
myBorg.GetDriveFaultA(log);
myBorg.AllStop(log);
myBorg.SetMotorB(128, log);
System.Threading.Thread.Sleep(1000);
int gety = myBorg.GetMotorB(log);
log.WriteLog("Reporting speed B: " + gety.ToString());
myBorg.GetDriveFaultB(log);
myBorg.AllStop(log);
myBorg.SetMotorA(-128, log);
System.Threading.Thread.Sleep(1000);
getx = myBorg.GetMotorA(log);
log.WriteLog("Reporting speed A: " + getx.ToString());
myBorg.GetDriveFaultA(log);
myBorg.AllStop(log);
myBorg.SetMotorB(-128, log);
System.Threading.Thread.Sleep(1000);
gety = myBorg.GetMotorB(log);
log.WriteLog("Reporting speed B: " + gety.ToString());
myBorg.GetDriveFaultB(log);
myBorg.AllStop(log);
myBorg.SetAllMotors(128, log);
System.Threading.Thread.Sleep(1000);
myBorg.AllStop();
myBorg.SetAllMotors(-128, log);
System.Threading.Thread.Sleep(1000);
myBorg.AllStop();
myBorg.GetFailsafe(log);
myBorg.SetFailsafe(true, log);
myBorg.SetFailsafe(true, log);
myBorg.SetFailsafe(false, log);
myBorg.SetFailsafe(false, log);
myBorg.GetLED1(log);
myBorg.GetLED2(log);
myBorg.GetLEDBatteryMonitor(log);
myBorg.SetLEDBatteryMonitor(false, log);
myBorg.GetLEDBatteryMonitor(log);
myBorg.SetLED1(255, 255, 255, log);
System.Threading.Thread.Sleep(1000);
myBorg.SetLED1(0, 0, 0, log);
//for (int i = 0; i < 255; i = i + 10)
// {
// for (int j = 0; j < 255; j = j + 10)
// {
// for (int k = 0; k < 255; k = k + 10)
// {
// myBorg.SetLEDs(Convert.ToByte(i), Convert.ToByte(j), Convert.ToByte(k), log);
// }
// }
// }
log.WriteLog("Battery voltage: " + myBorg.GetBatteryVoltage(log).ToString());
myBorg.SetLEDBatteryMonitor(true, log);
log.WriteLog("Board ID: 0x" + myBorg.GetBoardID(log).ToString("X2"));
myBorg.GetBatteryMonitoringLimits(log);
myBorg.SetBatteryMonitoringLimits(7.0M, 35.0M, log);
}
private static void TestLEDs(ThunderBorg_class myBorg, Logger_class log)
{
log.WriteLog("------------------------------------------------------------------");
log.WriteLog("Starting LED test...");
log.WriteLog("Setting battery LED monitor to off...");
myBorg.SetLEDBatteryMonitor(false, log);
log.WriteLog();
log.WriteLog("Current battery LED monitoring setting: " + myBorg.GetLEDBatteryMonitor(log).ToString());
log.WriteLog();
log.WriteLog("Starting test");
log.WriteLog("Setting all LEDs to 255, 255, 255...");
myBorg.SetLEDs(255, 255, 255, log);
log.WriteLog("Getting LED values...");
log.WriteLog("LED1: " + myBorg.BytesToString(myBorg.GetLED1(log)));
log.WriteLog("LED2: " + myBorg.BytesToString(myBorg.GetLED2(log)));
log.WriteLog();
log.WriteLog("Sleeping for one second...");
System.Threading.Thread.Sleep(1000);
log.WriteLog("Setting LED1 to 0, 0, 0...");
myBorg.SetLED1(0, 0, 0, log);
log.WriteLog("LED1: " + myBorg.BytesToString(myBorg.GetLED1(log)));
log.WriteLog();
log.WriteLog("Sleeping for one second...");
System.Threading.Thread.Sleep(1000);
log.WriteLog("Setting LED2 to 0, 0, 0...");
myBorg.SetLED2(0, 0, 0, log);
log.WriteLog("LED2: " + myBorg.BytesToString(myBorg.GetLED2(log)));
log.WriteLog();
log.WriteLog("Sleeping for one second...");
System.Threading.Thread.Sleep(1000);
log.WriteLog("Setting all LEDs to 128, 128, 128...");
myBorg.SetLED1(128, 128, 128, log);
myBorg.SetLED2(128, 128, 128, log);
log.WriteLog("LED1: " + myBorg.BytesToString(myBorg.GetLED1(log)));
log.WriteLog("LED2: " + myBorg.BytesToString(myBorg.GetLED2(log)));
log.WriteLog();
log.WriteLog("Sleeping for one second...");
System.Threading.Thread.Sleep(1000);
log.WriteLog("Setting back to battery monitoring...");
myBorg.SetLEDBatteryMonitor(true, log);
log.WriteLog("Finished test.");
}