private void MotorSettingClicked(object sender, RoutedEventArgs routedEventArgs)
{
var control = sender as MotorControl;
if (control != null)
{
MotorSettings.SaveSettings += MotorSettings_SaveSettings;
_selectedMotorControl = control;
MotorSettings.Show(
control.BrickInputPort,
_brick.Ports[control.BrickInputPort].Type,
control.MotorMovementType,
control.DegreeMovement,
control.PowerRatingMovement,
control.TimeToMoveInSeconds);
}
}
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 Controller(MotorMixer mixer, AxesController pid, Gyro gyro, Radio radio, ControllerLoopSettings loopSettings, MotorSettings motorSettings, ILogger logger)
{
TelemetryData telemetryData = new TelemetryData();
long previousTime = DateTime.Now.Ticks;
long lastRadioTime = 0;
long lastSensorTime = 0;
long lastControlTime = 0;
long lastMotorTime = 0;
long lastTelemetryTime = 0;
bool systemArmed = false;
while (true)
{
long currentTime = DateTime.Now.Ticks;
if (currentTime >= (lastRadioTime + loopSettings.RadioLoopPeriod))
{
//Debug.Print((loopSettings.LoopUnit/(float)(currentTime-lastRadioTime)).ToString());
lastRadioTime = currentTime;
radio.Update();
systemArmed = radio.Throttle > motorSettings.MinimumOutput;
if (!systemArmed)
{
logger.Flush();
}
}
currentTime = DateTime.Now.Ticks;
if (systemArmed && (currentTime >= (lastSensorTime + loopSettings.SensorLoopPeriod)))
{
//Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastSensorTime)).ToString());
lastSensorTime = currentTime;
gyro.Update();
}
currentTime = DateTime.Now.Ticks;
if (systemArmed && (currentTime >= (lastControlTime + loopSettings.ControlAlgorithmPeriod)))
{
//Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastControlTime)).ToString());
lastControlTime = currentTime;
pid.Update(radio.Axes, gyro.Axes, (float)(currentTime - previousTime) / loopSettings.LoopUnit);
previousTime = currentTime;
}
currentTime = DateTime.Now.Ticks;
if (currentTime >= (lastMotorTime + loopSettings.MotorLoopPeriod))
{
//Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastMotorTime)).ToString());
if (systemArmed)
{
mixer.Update(radio.Throttle, pid.Axes);
}
else
{
mixer.SetSafe();
}
lastMotorTime = currentTime;
}
currentTime = DateTime.Now.Ticks;
if (systemArmed && (currentTime >= (lastTelemetryTime + loopSettings.TelemetryLoopPeriod)))
{
//Debug.Print((loopSettings.LoopUnit / (float)(currentTime - lastTelemetryTime)).ToString());
telemetryData.Update(gyro.Axes, radio.Axes, pid.Axes, currentTime);
lastTelemetryTime = currentTime;
logger.Write(telemetryData);
}
}
}
internal VelocityMotorSettings1D(MotorSettings motorSettings)
: base(motorSettings)
{
}
internal VelocityMotorSettings(MotorSettings motorSettings)
{
this.motorSettings = motorSettings;
}
public PWMMotor(PWM pwmOutput, MotorSettings settings)
{
_pwmOutput = pwmOutput;
_settings = settings;
SetSafe();
}
internal ServoSettings1D(MotorSettings motorSettings)
: base(motorSettings)
{
}