Esempio n. 1
0
 public IMU(Accelerometer a, Gyroscope g)
 {
     oAcc = a;
     oGyro = g;
 }
Esempio n. 2
0
        public static void Main()
        {
            // Inizializzazione ingressi
            analogIn[0] = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An0);
            analogIn[1] = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An1);
            analogIn[2] = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An2);
            analogIn[3] = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An3);
            analogIn[4] = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An4);
            analogIn[5] = new AnalogIn((AnalogIn.Pin)FEZ_Pin.AnalogIn.An5);

            //button[(int)Button.menu] = new InterruptPort((Cpu.Pin)FEZ_Pin.Interrupt.Di11, true, Port.ResistorMode.PullUp, Port.InterruptMode.InterruptEdgeLow);
            //button[(int)Button.enter] = new InterruptPort((Cpu.Pin)FEZ_Pin.Interrupt.Di34, true, Port.ResistorMode.PullUp, Port.InterruptMode.InterruptEdgeLow);
            //button[(int)Button.up] = new InterruptPort((Cpu.Pin)FEZ_Pin.Interrupt.Di32, true, Port.ResistorMode.PullUp, Port.InterruptMode.InterruptEdgeLow);
            //button[(int)Button.down] = new InterruptPort((Cpu.Pin)FEZ_Pin.Interrupt.Di30, true, Port.ResistorMode.PullUp, Port.InterruptMode.InterruptEdgeLow);

            // Inizializzazione LCD
            //var lcdProvider = new GpioLcdTransferProvider((Cpu.Pin)FEZ_Pin.Digital.Di2, (Cpu.Pin)FEZ_Pin.Digital.Di3,
            //    (Cpu.Pin)FEZ_Pin.Digital.Di4, (Cpu.Pin)FEZ_Pin.Digital.Di5, (Cpu.Pin)FEZ_Pin.Digital.Di6,
            //    (Cpu.Pin)FEZ_Pin.Digital.Di7);
            //myLcd = new Lcd(lcdProvider);
            //myLcd.Begin(16, 2);

            // Init uscite
            led = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.LED, false);
            MotorA = new Motor(PWM.Pin.PWM2, (Cpu.Pin)FEZ_Pin.Digital.Di8, 25000);
            MotorB = new Motor(PWM.Pin.PWM5, (Cpu.Pin)FEZ_Pin.Digital.Di4, 25000);

            // Init sensore e PID
            Acc = new Accelerometer();
            Gyro = new Gyroscope();
            Imu = new IMU(Acc,Gyro);
            PidA = new PID(1, 0 , 0, -90, 90, 0, 100);
            PidB = new PID(1, 0, 0, -90, 90, 0, 100);

            PidA.Deadzone = 12;
            PidB.Deadzone = 15;

            PidA.SetMode(PID.Mode.Manual);
            PidB.SetMode(PID.Mode.Manual);

            Acc.Invert = new int[] { -1, -1, -1 };
            Gyro.Invert = new int[] { -1, -1 };
            Acc.Offset = new float[] { 1656, 1579, 1650 };      // forse sono da mettere come sopra...
            Gyro.Offset = new float[] { 1328, 1331 };

            // Init porta seriale e parser
            UART = new SerialPort("COM2", 115200);
            UART.ReadTimeout = 200;
            UART.Open();
            UART.DataReceived += new SerialDataReceivedEventHandler(UART_DataReceived);

            Parser = new StringParser();
            Parser.addCommand("ping", Parser_onPing);
            Parser.addCommand("setpoint", Parser_onSetPoint);
            Parser.addCommand("pid", Parser_onPid);
            Parser.addCommand("mt", Parser_onMotorTest);
            Parser.addCommand("pidstop", Parser_onPidStop);
            Parser.addCommand("pidstart", Parser_onPidStart);
            Parser.addCommand("rq", Parser_onSerialMonitor);

            // Eventi interrupt
            //button[(int)Button.menu].OnInterrupt += new NativeEventHandler(menuBut_OnInterrupt);
            //button[(int)Button.enter].OnInterrupt += new NativeEventHandler(enterBut_OnInterrupt);

            // Definizione timer
            Timer sensacq_timer = new Timer(new TimerCallback(SensAcq), null, 0, 10);
            Timer control_timer = new Timer(new TimerCallback(Control), null, 0, 20);
            Timer display_timer = new Timer(new TimerCallback(Display), null, 0, 50);

            Thread.Sleep(Timeout.Infinite);
        }