public IMU(Accelerometer a, Gyroscope g)
{
oAcc = a;
oGyro = g;
}
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);
}