public static void ResetMotorHat()
{
DC1Helper = null;
DC2Helper = null;
DC3Helper = null;
DC4Helper = null;
mh = null;
driver = null;
driver = new I2cDriver(ConnectorPin.P1Pin3.ToProcessor(), ConnectorPin.P1Pin5.ToProcessor());
mh = new Motor_Hat(driver, 0x60, 50);
DC1Helper = new MotorHelper(mh.GetMotor(1));
DC2Helper = new MotorHelper(mh.GetMotor(2));
DC3Helper = new MotorHelper(mh.GetMotor(3));
DC4Helper = new MotorHelper(mh.GetMotor(4));
Console.WriteLine("Resetted Motor Hat");
}
public static void motorCalcs(float speed, float wheelPos1, float wheelPos2, int freq = 500)
{
Motor_Hat mh = new Motor_Hat (0x60, freq);
var DC1 = mh.GetMotor (1);
var DC2 = mh.GetMotor (2);
var DC1Helper = new MotorHelper (DC1);
var DC2Helper = new MotorHelper (DC2);
int DC1motorSpeed = 0, DC2motorSpeed = 0;
if (speed > 0) {
DC1motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) - (wheelPos1 * 2.55)));
DC2motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) - (wheelPos2 * 2.55)));
} else if (speed < 0) {
DC1motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) + (wheelPos1 * 2.55)));
DC2motorSpeed = Convert.ToInt32 (Math.Floor ((speed * 2.55) + (wheelPos2 * 2.55)));
}
if(speed == 0){
if(wheelPos1 > 0){
DC1motorSpeed = Convert.ToInt32(Math.Floor((wheelPos1 * 2.55) * -1 + 1));
DC2motorSpeed = Convert.ToInt32(Math.Floor((wheelPos1 * 2.55)));
}
if(wheelPos2 > 0){
DC1motorSpeed = Convert.ToInt32(Math.Floor((wheelPos2 * 2.55)));
DC2motorSpeed = Convert.ToInt32(Math.Floor((wheelPos2 * 2.55) * -1 + 1));
}
}
Console.WriteLine ( "Left motor: {0}\n" +
"Right motor: {1}\n", DC1motorSpeed, DC2motorSpeed);
if (DC1motorSpeed > 255 || DC2motorSpeed > 255) {
DC1motorSpeed -= 255;
DC2motorSpeed -= 255;
}
if(DC1motorSpeed < -255 || DC2motorSpeed < -255) {
DC1motorSpeed += 255;
DC2motorSpeed += 255;
}
if(DC1motorSpeed < 0) DC1Helper.Backward(DC1motorSpeed);
if(DC1motorSpeed > 0) DC1Helper.Forward(DC1motorSpeed);
if(DC1motorSpeed == 0) DC1Helper.Stop();
if(DC2motorSpeed < 0) DC2Helper.Backward(DC2motorSpeed);
if(DC2motorSpeed > 0) DC2Helper.Forward(DC2motorSpeed);
if(DC2motorSpeed == 0) DC2Helper.Stop();
}
public static void Main(string[] args)
{
Process proc = new Process();
proc.StartInfo.FileName = "cat";
proc.StartInfo.Arguments = "/sys/class/thermal/thermal_zone0/temp";
proc.StartInfo.UseShellExecute = false;
proc.StartInfo.RedirectStandardError = true;
proc.StartInfo.RedirectStandardInput = true;
proc.StartInfo.RedirectStandardOutput = true;
proc.Start();
var output = proc.StandardOutput.ReadToEnd ();
Console.WriteLine("stdout: {0}", output);
if(Helpers.IsLinux) mh = new Motor_Hat();
socket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
socket.Bind(new IPEndPoint(0, BattleBotServerPort));
logWriter = LogWriter.Instance;
while (true)
{
socket.Listen(100);
Console.WriteLine("Awaiting Data");
accepted = socket.Accept();
if (clientIP == null)
{
IPEndPoint clientIPEndPoint = accepted.RemoteEndPoint as IPEndPoint;
clientIP = clientIPEndPoint.Address;
Thread serverToClientThread = new Thread(new ThreadStart(SocketServerToClient));
serverToClientThread.Start();
}
Buffer = new byte[accepted.SendBufferSize];
int bytesRead = accepted.Receive(Buffer);
byte[] formatted = new byte[bytesRead];
for (int i = 0; i < bytesRead; i++)
{
formatted[i] = Buffer[i];
}
string strData = Encoding.ASCII.GetString(formatted);
string[] typcom = strData.Split(':');
if (typcom.Length == 2 || typcom.Length == 3 ) {
string commandtype = typcom[0];
string command = typcom[1];
string parameters = null;
if (typcom.Length == 3) {
parameters = typcom[2];
}
Console.WriteLine(strData + Environment.NewLine);
if(commandtype == "MC") // We are dealing with motor commands.
{
string[] temp = command.Split(',');
if (temp.Length == 1)
{
speed = float.Parse(temp[0]);
wheelPos1 = 0;
wheelPos2 = 0;
freq = 500;
}
else if(temp.Length == 4)
{
speed = float.Parse(temp[0]);
wheelPos1 = float.Parse(temp[1]);
wheelPos2 = float.Parse(temp[2]);
freq = int.Parse (temp [3]);
}
else
{
Console.WriteLine("Error, can't parse motor command \""+command+"\"");
}
}
if(commandtype == "L") // We are dealing with something we need to log server sided.
{
Console.WriteLine("LOG: "+command);
if(parameters != null)
{
logWriter.WriteToLog(parameters, 123.23);
}
}
if (commandtype == "C") // We are dealing with a system command. The array can be more than 2 if parameters are added
{
if (command == "shutdown")
{
Console.WriteLine("Ima shutting down");
}
if (command == "reboot")
{
Console.WriteLine("REEEEEEEEEEBOOT");
}
if(command == "exit")
{
break;
}
}
Console.WriteLine(Environment.NewLine);
}
else
{
Console.WriteLine("Can't parse \"" + strData + "\" as a command.");
}
if(Helpers.IsLinux) Helpers.motorCalcs (speed, wheelPos1, wheelPos2, freq);
Thread.Sleep(sleepTime);
}
socket.Close();
accepted.Close();
}