Ejemplo n.º 1
0
 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");
 }
Ejemplo n.º 2
0
        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();
        }
Ejemplo n.º 3
0
        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();
        }