public static void Main()
{
//Initialize motors first
MotorSettings motorSettings = GetMotorSettings();
Motor frontMotor = new PWMMotor(new PWM((Cpu.Pin) FezPin.PWM.Di8, 4000000), motorSettings);
Motor rearMotor = new PWMMotor(new PWM((Cpu.Pin) FezPin.PWM.Di10, 4000000), motorSettings);
Motor leftMotor = new PWMMotor(new PWM((Cpu.Pin) FezPin.PWM.Di9, 4000000), motorSettings);
Motor rightMotor = new PWMMotor(new PWM((Cpu.Pin) FezPin.PWM.Di6, 4000000), motorSettings);
MotorMixer mixer = new QuadMixer(frontMotor, rearMotor, leftMotor, rightMotor);
//Telemetry
ISDCard sdCard = (ISDCard) new SDCard();
if (Configuration.DebugInterface.GetCurrent() == Configuration.DebugInterface.Port.USB1)
sdCard.MountFileSystem();
else
new TelemetryPresenter(sdCard,(Cpu.Pin) FezPin.Digital.LED);
ILogger logger = new PersistenceWriter(new FileStream(@"\SD\telemetry.bin",FileMode.OpenOrCreate), new TelemetryFormatter());
//Sensors
I2CBus twiBus = new I2CBus();
Gyro gyro = new ITG3200(twiBus, GetGyroFactor());
Radio radio = new DefaultRadio(twiBus, GetRadioSettings());
//Control Algoriths
PIDSettings[] pidSettings = GetPIDSettings();
AxesController axesController = new AxesController(pidSettings[0], pidSettings[1], pidSettings[2], true);
ControllerLoopSettings loopSettings = GetLoopSettings();
Controller controller = new Controller(mixer, axesController, gyro, radio, loopSettings, GetMotorSettings(),logger);
//controller.Start();
}
public static void Main()
{
SDCard sdCard = new SDCard();
//todo: need sdCard mode detection
ILogger logger = new PersistenceWriter(new FileStream(@"\SD\telemetry.bin", FileMode.CreateNew), new TelemetryFormatter());
//ILogger logger = new DebugLogger(); // use for debugging
//ILogger logger = new NullLogger(); // use for release
//Assumes one type of speed controller on quad
//ISpeedControllerSettings speedControllerSettings = new HobbyKingSs2530Settings();
//IPWM pwmFront = new SecretLabsPWM(Pins.GPIO_PIN_D9);
//SpeedController electricSpeedControllerFront = new SpeedController(speedControllerSettings, pwmFront);
//IPWM pwmRear = new SecretLabsPWM(Pins.GPIO_PIN_D8);
//SpeedController electricSpeedControllerRear = new SpeedController(speedControllerSettings, pwmRear);
//IPWM pwmRight = new SecretLabsPWM(Pins.GPIO_PIN_D8);
//SpeedController electricSpeedControllerRight = new SpeedController(speedControllerSettings, pwmRight);
//IPWM pwmLeft = new SecretLabsPWM(Pins.GPIO_PIN_D8);
//SpeedController electricSpeedControllerLeft = new SpeedController(speedControllerSettings, pwmLeft);
MotorMixer mixer = null;
PIDSettings[] pidSettings = GetPIDSettings();
AxesController axesController = new AxesController(pidSettings[0], pidSettings[1], pidSettings[2], true);
Gyro gyro = new DefaultGyro(new AircraftPrincipalAxes());
Radio radio = new DefaultRadio(null,null);
ControllerLoopSettings loopSettings = GetLoopSettings();
Controller controller = new Controller(mixer, axesController, gyro, radio, loopSettings, GetMotorSettings(), logger);
}
public static void Main()
{
Motor testMotor = new TestMotor();
MotorMixer mixer = new QuadMixer(testMotor, testMotor, testMotor, testMotor);
ILogger logger = new PersistenceWriter(new MemoryStream(), new TelemetryFormatter());
Gyro gyro = new TestGyro();
Radio radio = new TestRadio();
PIDSettings[] pidSettings = GetPIDSettings();
AxesController axesController = new AxesController(pidSettings[0], pidSettings[1], pidSettings[2], true);
ControllerLoopSettings loopSettings = GetLoopSettings();
Controller controller = new Controller(mixer, axesController, gyro, radio, loopSettings, GetMotorSettings(), logger);
}
public override void Close()
{
LinkManager.Manager.RemoveListener(this, ComponentEventListener);
if (RecorderScope != null)
{
RecorderScope.Close();
RecorderScope = null;
}
if (Controller != null)
{
Controller.Close();
Controller = null;
}
base.Close();
}
public ThreadingProgram()
{
Gyro gyro = new TestGyro();
Sensors sensors = new Sensors(gyro);
Motor testMotor = new TestMotor();
MotorMixer mixer = new QuadMixer(testMotor, testMotor, testMotor, testMotor);
ILogger logger = new PersistenceWriter(new MemoryStream(), new TelemetryFormatter());
Radio radio = new TestRadio();
PIDSettings[] pidSettings = GetPIDSettings();
AxesController axesController = new AxesController(pidSettings[0], pidSettings[1], pidSettings[2], true);
_loopSettings = GetLoopSettings();
Timer sensorTimer = new Timer(tester, sensors,new TimeSpan(0),new TimeSpan((long)_loopSettings.SensorLoopFrequency) );
}
private void PreInit()
{
CheckAxisPending = true;
AxisOptions = null;
TriggerOptions = null;
RecorderMode = RecorderModeKinds.RecorderModeLocalOnly;
LastRecorderMode = RecorderModeKinds.RecorderModeIdle;
CurrentSystemModeId = 0;
LastSystemModeId = 0xff;
OptionsTab = new Options(this);
AxesTab = new Axes(this);
TriggersTab = new Triggers(this);
JobStorageTab = new JobStorage(this);
MotionTab = new Motion(this);
RecorderScope = new RecorderChart(this);
Controller = new AxesController(this);
}
public TestController(MotorMixer mixer, AxesController pid, Gyro gyro, Radio radio, ControllerLoopSettings loopSettings, MotorSettings motorSettings, ILogger logger)
{
long previousTime = DateTime.Now.Ticks;
long lastRadioTime = 0;
while (true)
{
long currentTime = DateTime.Now.Ticks;
if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod))
{
Debug.Print((loopSettings.LoopUnit/(float) (currentTime - lastRadioTime)).ToString());
lastRadioTime = currentTime;
radio.Update();
gyro.Update();
pid.Update(radio.Axes, gyro.Axes, (float) (currentTime - lastRadioTime)/loopSettings.LoopUnit);
mixer.Update(radio.Throttle, pid.Axes);
}
}
}
