public static void Main()
{
//var aaa = new Class1();
//aaa.TestX();
using (var transmitter = new Transmitter())
{
//Firts vehiclle
//var receiverTrain = new Receiver(transmitter, Channel.Ch1);
//var motor = new Motor(receiverTrain.RedConnector);
//var light = new Led(receiverTrain.RedConnector);
//Rover on another channel
var receiverRover = new Receiver(transmitter, Channel.Ch2);
var drive = new Motor(receiverRover.BlueConnector);
var steeringWheel = new Servo(receiverRover.RedConnector);
//Now Control
while (true)
{
//motor.IncSpeed();
//light.TurnOn();
drive.SetSpeed(100);
steeringWheel.SetAngle180(45);
Thread.Sleep(1000);
steeringWheel.Center();
//motor.Brake();
//light.TurnOff();
}
}
}
public void TestSmartDashboardType()
{
using (Servo s = NewServo())
{
Assert.That(s.SmartDashboardType, Is.EqualTo("Servo"));
}
}
public async void Run(IBackgroundTaskInstance taskInstance)
{
deferral = taskInstance.GetDeferral();
taskInstance.Canceled += TaskInstance_Canceled;
shield = await FEZUtilityShield.CreateAsync().ConfigureAwait(false);
servo = new Servo(new PCA9685PWMChannel(shield.PCA9685PWM, (int)FEZUtilityShield.PwmPin.P0));
servo.SetLimits(650, 2750, 0, 180, 90);
servo.Disengage();
int i = 130;
servo.Position = i;
await Task.Delay(500);
while (i <= 180)
{
servo.Position = i;
i += 2;
await Task.Delay(100);
}
servo.Disengage();
}
static void Main(string[] args)
{
try
{
string portName = SerialPortHelper.FindProlificPortName();
using (SSC32Board board = new SSC32Board(portName))
{
Servos servos = new Servos();
servos.ConfigureFromFile("ConfigSSC32.cfg");
Servo baseServo = servos.BaseServo;
System.Console.WriteLine(baseServo);
baseServo.Move(-30, 100, board);
baseServo.Move(30, 100, board);
}
}
catch (Exception ex)
{
System.Console.WriteLine(ex.ToString());
}
finally
{
System.Console.WriteLine("Hit <Enter> to end.");
System.Console.ReadLine();
}
}
/// <summary>
/// 75個のデータを受けて、サーボ情報を調べます。
/// </summary>
/// <param name="servoStrings"></param>
/// <returns></returns>
PoseFrame ParseOneFrame(string[] servoStrings)
{
if (servoStrings.Length != 80)
{
Debug.LogError("不正なフレームです");
}
PoseFrame ret = new PoseFrame();
ret.commandLength = servoStrings[0];
ret.command = servoStrings[1];
ret.commandPadding = servoStrings[2];
ret.frameWait = servoStrings[3];
ret.wait = HexToInt(ret.frameWait);
//25軸だと信じていますよ
for (int i = 0; i < 25; i++)
{
Servo tmp = new Servo();
tmp.id = servoStrings[4 + i * 3];
tmp.hb = servoStrings[4 + i * 3 + 1];
tmp.lb = servoStrings[4 + i * 3 + 2];
tmp.servoValue = ServoStringToValue(servoStrings[4 + (i * 3) + 1], servoStrings[4 + (i * 3) + 2]);
tmp.eulerAngle = (tmp.servoValue - 7500) * 0.03375f; //0.03375= 135/4000
ret.servos.Add(tmp);
}
//checkbyte
ret.checkByte = servoStrings[79];
return(ret);
}
//Initialization code
private void Form1_Load(object sender, EventArgs e)
{
servo1 = new Servo();
servo2 = new Servo();
controllerCmb.Enabled = false;
servoNumCombo.Enabled = false;
positionTrk.SetRange(-23, 232);
servo1.Attach += new AttachEventHandler(servo_Attach);
servo1.Detach += new DetachEventHandler(servo_Detach);
servo1.Error += new ErrorEventHandler(servo_Error);
servo1.PositionChange += new PositionChangeEventHandler(servo_PositionChange);
servo2.Attach += new AttachEventHandler(servo_Attach);
servo2.Detach += new DetachEventHandler(servo_Detach);
servo2.Error += new ErrorEventHandler(servo_Error);
servo2.PositionChange += new PositionChangeEventHandler(servo_PositionChange);
servo1.open(serialNum1);
servo2.open(serialNum2);
}
private void ServoSelector_SelectedIndexChanged(object sender, EventArgs e)
{
bool enabled = false;
object obj = ServoSelector.SelectedItem;
if ((obj != null) && (obj is ServoWrapper wrapper))
{
Servo servo = wrapper.Servo;
enabled = true;
MinPulseUpDown.ValueChanged -= MinPulseUpDown_ValueChanged;
MaxPulseUpDown.ValueChanged -= MaxPulseUpDown_ValueChanged;
{
MinPulseUpDown.Value = servo.PWM.MinimumPulse;
MaxPulseUpDown.Value = servo.PWM.MaximumPulse;
}
MinPulseUpDown.ValueChanged += MinPulseUpDown_ValueChanged;
MaxPulseUpDown.ValueChanged += MaxPulseUpDown_ValueChanged;
settings.ServoRanges[wrapper.Name] = servo.PWM.PulseRange;
}
EnableBtn.Enabled = enabled;
DisableBtn.Enabled = enabled;
MinPulseUpDown.Enabled = enabled;
MaxPulseUpDown.Enabled = enabled;
}
public void TestServoInitialized()
{
using (Servo s = NewServo())
{
Assert.That(SimData.PWM[2].GetInitialized());
}
}
public static void SetMotorDuty(Servo servo, double speedPercentage, Direction direction)
{
if (speedPercentage < 0 || speedPercentage > 100)
{
throw new ArgumentOutOfRangeException("speedPercentage", "Must be between 0 and 100 %");
}
if (servo == Servo.A)
{
if (direction == Direction.Back)
{
DIRA.Write(GpioPinValue.Low);
}
else
{
DIRA.Write(GpioPinValue.High);
}
PWM1.SetDesiredFrequency(5000);
PWMA.Start();
PWMA.SetActiveDutyCyclePercentage(speedPercentage / 100);
}
else
{
if (direction == Direction.Forvard)
{
DIRB.Write(GpioPinValue.High);
}
else
{
DIRB.Write(GpioPinValue.Low);
}
PWM3.SetDesiredFrequency(5000);
PWMB.Start();
PWMB.SetActiveDutyCyclePercentage(speedPercentage / 100);
}
}
public void TestServoStarts0()
{
using (Servo s = NewServo())
{
Assert.AreEqual(s.Get(), 0);
}
}
public bool IsResting(Servo servo)
{
var queryRestingServos = this.ServoRests.Where(rs => rs.Servo == servo);
if (queryRestingServos.Any())
{
//we have a rest assigned for this servo
//remove all rests which are stale
this.ServoRests.RemoveAll(rs => DateTime.Now - rs.RestTimeBegan > ServoRestManager.RestingTime);
//check if we still ahve one for this servo
var newQueryRestingServos = this.ServoRests.Where(rs => rs.Servo == servo);
if (newQueryRestingServos.Any())
{
return(true);
}
else
{
//wake it up
servo.WriteValue(RegisterType.TorqueEnable, 1);
//no longer resting
return(false);
}
}
else
{
return(false);
}
}
/// <summary>
///
/// </summary>
/// <param name="gearPwm">PWM port for controlling servo</param>
public GearBox(IPwmPort gearPwm)
{
pwm = gearPwm;
ServoConfig config = new ServoConfig(new Angle(0), new Angle(180), 500, 2500, 50);
servo = new Servo(pwm, config);
}
/*
*
#region Demo 2 - Custom heartbeat
*
* static async Task Demo2()
* {
* //await mcu.DigitalWrite(0, true);
* //await mcu.DigitalWrite(0, false);
*
* //await mcu.PinMode(21, true);
* //var foo = await mcu.AnalogRead(21);
* //await mcu.Execute(Brief.Compile("[42 event( 21 analogRead edata )event] setLoop")); // TODO: dangerous!!
* await mcu.Execute(OldBrief.Compile("[loopTicks 10 mod 0 = [42 event( 20 analogRead edata 21 analogRead edata )event] [] choice] setLoop")); // TODO: dangerous!!
* const int wait = 5000;
* await Task.Delay(wait);
* await mcu.Execute(OldBrief.Compile("stopLoop"));
* await Task.Delay(1000); // let console catch up
* Console.WriteLine();
* Console.WriteLine("Frames/sec = {0} (dropped {1})", (float)frames / (float)wait * 1000.0, dropped);
* //await mcu.Execute(Brief.Compile("40 loopTicks event"));
* //await Task.Delay(1000); // give time for event
* }
*
#endregion Demo 2
*
#region Demo 3 - MonsterMoto driver
*
* static async Task Demo3()
* {
* var left = new Motor(mcu, 8, 7, 5, "left");
* await left.Initialize();
*
* var right = new Motor(mcu, 9, 4, 6, "right");
* await right.Initialize();
*
* await left.SetSmoothing(1);
* await left.SetTargetPwmPercentage(0.92);
* await left.SetTargetPwmPercentage(0.21);
* await left.SetTargetPwmPercentage(-0.54);
* await left.SetTargetPwmPercentage(0.0);
* await left.SetSmoothing(2);
* await left.SetTargetPwmPercentage(1);
* await left.Brake();
*
* await right.SetTargetPwmPercentage(-1);
* await right.Brake();
*
* await mcu.Execute(OldBrief.Compile("left 255 drive right -255 drive"));
* await mcu.Execute(OldBrief.Compile("left 50 drive right 50 drive"));
* await mcu.Execute(OldBrief.Compile("left stop right stop"));
* }
*
#endregion Demo 3
*
#region Demo 4 - Pololu Gyro
*
* static async Task Demo4()
* {
* var gyro = new Gyro(mcu, "gyro");
* await gyro.Initialize();
*
* for (var i = 0; i < 10; i++)
* {
* Console.WriteLine("Gyro: {0}", await gyro.AngularSpeedRaw());
* await Task.Delay(100);
* }
*
* var test = 0.0;
* for (var i = 0; i < 10; i++)
* {
* var speed = await gyro.AngularSpeedDps();
* test += speed;
* Console.WriteLine("GyroDps: {0} ({1})", speed, test);
* await Task.Delay(100);
* }
*
* await mcu.Execute(OldBrief.Compile("[42 gyroDps event 100 delay] setLoop")); // TODO: dangerous!!
* await Task.Delay(1000);
* await mcu.Execute(OldBrief.Compile("stopLoop"));
* }
*
#endregion Demo 4
*
#region Demo 5 - Pololu Accelerometer
*
* static async Task Demo5()
* {
* var accelerometer = new Accelerometer(mcu, "acceleration");
* await accelerometer.Initialize();
*
* for (var i = 0; i < 1000; i++)
* {
* Console.WriteLine("Acceleration: {0}", await accelerometer.Acceleration());
* await Task.Delay(100);
* }
*
* await mcu.Execute(OldBrief.Compile("[42 acceleration event 100 delay] setLoop")); // TODO: dangerous!!
* await Task.Delay(1000);
* await mcu.Execute(OldBrief.Compile("stopLoop"));
* }
*
#endregion Demo 5
*
#region Demo 6 - Balancing
*
* static async Task Demo6()
* {
* var balancing = new Balancing(mcu);
* await balancing.Initialize();
*
* var lastY = float.NaN;
* var lastX = float.NaN;
* var lastStart = false;
* var lastA = false;
*
#if SHORT_BOT
* var parameters = new Tuple<string, double>[] {
* new Tuple<string, double>("MinLeft", 30),
* new Tuple<string, double>("MinRight", 20),
* new Tuple<string, double>("SPD", 0),
* new Tuple<string, double>("Trim", -0.3),
* new Tuple<string, double>("PA", 30),
* new Tuple<string, double>("IA", 0.95),
* new Tuple<string, double>("PM", 0.9),
* new Tuple<string, double>("IM", 0.01),
* new Tuple<string, double>("DM", 10) };
#else
* var parameters = new Tuple<string, double>[] {
* new Tuple<string, double>("MinLeft", 20),
* new Tuple<string, double>("MinRight", 20),
* new Tuple<string, double>("SPD", 0),
* new Tuple<string, double>("Trim", 0.6),
* new Tuple<string, double>("PA", 30),
* new Tuple<string, double>("IA", 0.95),
* new Tuple<string, double>("PM", 0.9),
* new Tuple<string, double>("IM", 0.01),
* new Tuple<string, double>("DM", 10) };
#endif
* var selected = -1;
* GamePadState pad;
*
* foreach (var p in parameters)
* await mcu.Execute(OldBrief.Compile("{0} set{1}", (int)(p.Item2 * 1000 + 0.5), p.Item1));
*
* bool allowParameterTweaks = false;
* bool exit = false;
* var dampenDrive = 1.0f;
*
* do
* {
* pad = GamePad.GetState(PlayerIndex.One);
*
* var y = pad.ThumbSticks.Left.Y;
*
* const double easingThreshold = 0.2;
* if (Math.Abs(y) < easingThreshold)
* dampenDrive = 1.0f; // turn off dampening once user eases off
* y *= dampenDrive;
* dampenDrive *= (Math.Abs(y) > easingThreshold ? 0.95f : 1.0f); // increase dampening until eased off
*
* if (y.CompareTo(lastY) != 0)
* {
* lastY = y;
* Console.WriteLine("Y: {0}", y);
#if SHORT_BOT
* var d = y * (float)(y > 0 ? 2.75 : 3.5);
#else
* var d = y * (float)(y > 0 ? 1.75 : 1.5);
#endif
* await balancing.SetDrive(d);
* //Console.WriteLine("Drive: {0}", d);
* }
*
* var x = pad.ThumbSticks.Left.X;
* if (x.CompareTo(lastX) != 0)
* {
* lastX = x;
* var t = x * 50;
#if !SHORT_BOT
* t = -t; // motors reversed
#endif
* await balancing.SetTurn(t);
* Console.WriteLine("Turn: {0}", t);
* }
*
* if (allowParameterTweaks)
* {
* var s = pad.IsButtonDown(Buttons.Start);
* if (s && !lastStart)
* {
* await balancing.ToggleBalance();
* Console.WriteLine("Toggle balancing");
* }
* lastStart = s;
*
* var a = pad.IsButtonDown(Buttons.A);
* if (a && !lastA)
* {
* selected = (selected + 1) % parameters.Length;
* Console.WriteLine("Selected: {0}", parameters[selected]);
* }
* lastA = a;
*
* if (pad.IsButtonDown(Buttons.X))
* {
* for (var i = 0; i < parameters.Length; i++)
* {
* var n = parameters[i].Item1;
* parameters[i] = new Tuple<string, double>(n, 0);
* await mcu.Execute(OldBrief.Compile("0 set{0}", n));
* }
* Console.WriteLine("Reset parameters");
* }
*
* var adjust = 0.0;
* var fine = pad.IsButtonDown(Buttons.Y);
* if (pad.IsButtonDown(Buttons.DPadUp))
* adjust = fine ? 0.01 : 0.1;
* if (pad.IsButtonDown(Buttons.DPadDown))
* adjust = fine ? -0.01 : -0.1;
*
*
* if (adjust != 0.0 && selected != -1)
* {
* var p = parameters[selected];
* var n = p.Item1;
* var v = p.Item2 + adjust;
* parameters[selected] = new Tuple<string, double>(n, v);
* await mcu.Execute(OldBrief.Compile("{0} set{1}", (int)(v * 1000 + 0.5), n));
* Console.WriteLine("{0} = {1}", n, v);
* await Task.Delay(100);
* }
*
* if (pad.IsButtonDown(Buttons.Back))
* exit = true;
* }
*
* if (pad.IsButtonDown(Buttons.LeftShoulder) &&
* pad.IsButtonDown(Buttons.RightShoulder) &&
* pad.Triggers.Left == 1 &&
* pad.Triggers.Right == 1 &&
* pad.IsButtonDown(Buttons.LeftStick) &&
* pad.IsButtonDown(Buttons.B))
* {
* allowParameterTweaks = !allowParameterTweaks;
* Console.WriteLine("Allow Parameter Tweaks: {0}", allowParameterTweaks);
* Thread.Sleep(500);
* }
*
* Thread.Sleep(10);
* } while (!exit);
* }
*
#endregion Demo 6
*/
#region Demo 7 - Servo
static async Task Demo7()
{
var pan = new Servo(mcu, 1, 1000, 2000);
await pan.Initialize();
var tilt = new Servo(mcu, 0, 1000, 2000);
await tilt.Initialize();
var lastY = float.NaN;
var lastX = float.NaN;
GamePadState pad;
do
{
pad = GamePad.GetState(PlayerIndex.One);
var y = pad.ThumbSticks.Left.Y;
var x = pad.ThumbSticks.Left.X;
if (x.CompareTo(lastX) != 0)
{
lastX = x;
Console.WriteLine("X: {0}", x);
await pan.Write(x);
}
if (y.CompareTo(lastY) != 0)
{
lastY = y;
Console.WriteLine("Y: {0}", y);
await tilt.Write(y);
}
} while (true);
}
public MeadowApp()
{
var led = new RgbLed(Device, Device.Pins.OnboardLedRed, Device.Pins.OnboardLedGreen, Device.Pins.OnboardLedBlue);
led.SetColor(RgbLed.Colors.Red);
servo = new Servo(Device.CreatePwmPort(Device.Pins.D08), NamedServoConfigs.SG90);
servo.RotateTo(0);
rotaryEncoder = new RotaryEncoder(Device, Device.Pins.D02, Device.Pins.D03);
rotaryEncoder.Rotated += (s, e) =>
{
if (e.Direction == Meadow.Peripherals.Sensors.Rotary.RotationDirection.Clockwise)
{
angle++;
}
else
{
angle--;
}
if (angle > 180)
{
angle = 180;
}
else if (angle < 0)
{
angle = 0;
}
servo.RotateTo(angle);
};
led.SetColor(RgbLed.Colors.Green);
}
public static void Main()
{
Servo servo = new Servo(PWMChannels.PWM_PIN_D9, 500, 2400);
while (true)
{
try
{
for (int angle = 0; angle <= 180; angle++)
{
servo.Angle = angle;
Thread.Sleep(40);
}
for (int angle = 179; angle > 0; angle--)
{
servo.Angle = angle;
Thread.Sleep(40);
}
}
catch (Exception ex)
{
Debug.Print(ex.Message);
}
}
}
/// <summary>
/// check ray against all other servos and entities
/// </summary>
/// <param name="originalServo"></param>
/// <param name="path"></param>
/// <returns></returns>
private bool CheckIfClear(Servo originalServo, ref PathDetails path)
{
//create ray using servo + next destination
foreach (Entity entity in _entities)
{
if (!entity.Equals(originalServo))
{
path.BlockedMain |= entity.Intersection(path.MainRay, ref path.MainNear);
path.BlockedSide0 |= entity.Intersection(path.SideRay0, ref path.SideNear0);
path.BlockedSide1 |= entity.Intersection(path.SideRay1, ref path.SideNear1);
}
}
foreach (Person person in _people)
{
path.BlockedMain |= person.Intersection(path.MainRay, ref path.MainNear);
path.BlockedSide0 |= person.Intersection(path.SideRay0, ref path.SideNear0);
path.BlockedSide1 |= person.Intersection(path.SideRay1, ref path.SideNear1);
}
//Debug
_worldGrid.DrawCircleAt(path.Offset0, 0.1f, (Colors.DarkSlateGray));
_worldGrid.DrawCircleAt(path.Offset1, 0.1f, (Colors.Red));
_worldGrid.DrawCircleAt(path.Origin, 0.1f, (Colors.Fuchsia));
bool isBlocked = path.BlockedMain || path.BlockedSide0 || path.BlockedSide1;
return(!isBlocked);
}
public ServoApp()
{
pwm = Device.CreatePwmPort(Device.Pins.D05);
servo = new Servo(pwm, NamedServoConfigs.Ideal180Servo);
TestServo();
}
public MeadowApp()
{
Console.WriteLine("Initializing...");
servo = new Servo(Device.CreatePwmPort(Device.Pins.D05), NamedServoConfigs.Ideal180Servo);
TestServo();
}
public void Initialize()
{
IPWMOutput MotorPWM = PWMBBB.PWMDevice2.OutputA;
IPWMOutput ServoPWM = PWMBBB.PWMDevice0.OutputB;
this.MotorCtrl = new TalonMC(MotorPWM, MOTOR_MAX_SPEED);
this.DoorServo = new Servo(ServoPWM);
}
public void TestServoSetAngle()
{
using (Servo s = NewServo())
{
s.SetAngle(76.5);
Assert.AreEqual(s.GetAngle(), 76.5);
}
}
/// <summary>
/// <para>Gets a Servo object from the database based on the time of the measurement</para>
/// </summary>
/// <param name="dateTime">Date and time of the measurement</param>
/// <returns>
/// <para>Servo object serialized into JSON</para>
/// </returns>
public string getServo(DateTime dateTime)
{
List <Servo> servos = _context.Servo.Where(servo => servo.Date == dateTime).ToList(); //might work
Servo t = servos[0];
string s = JsonSerializer.Serialize(t);
return(s);
}
public void TestServeSetOutsideAngleOver()
{
using (Servo s = NewServo())
{
s.SetAngle(185.9);
Assert.AreEqual(s.GetAngle(), 180.0, .1);
}
}
public void TestServeSetOutsideAngleUnder()
{
using (Servo s = NewServo())
{
s.SetAngle(-10.5);
Assert.AreEqual(s.GetAngle(), 0.0, .1);
}
}
//private bool m_IsEnabled;
public ServoControlModel(Servo servo, SSC32Board board)
{
this.Servo = servo;
m_Board = board;
m_Position = 1500;
servo.Changed += new EventHandler(OnServoChanged);
}
//private bool m_IsEnabled;
public ServoControlModel(Servo servo, SSC32Board board)
{
this.Servo = servo;
m_Board = board;
m_Position = 1500;
servo.Changed += new EventHandler(OnServoChanged);
}
public void TestServoSet()
{
using (Servo s = NewServo())
{
s.Set(1.0);
Assert.AreEqual(s.Get(), 1.0);
}
}
public static BlockJsonInfo JsonObject(Block block, float[] origin = null)
{
if (origin == null)
{
origin = origin_base;
}
BlockJsonInfo jsonInfo = new BlockJsonInfo
{
name = block.Type.ToString(),
position = new float[3] {
block.Position.x - origin[0], block.Position.y - origin[1], block.Position.z - origin[2]
},
rotation = new float[3] {
block.Rotation.x, block.Rotation.y, block.Rotation.z
},
color = ColorSpaceUtility.UnquantizeToArray(block.Color),
scale = new float[3] {
block.Scale.x, block.Scale.y, block.Scale.z
},
};
// custom stats for special blocks
switch (block.Type)
{
case BlockIDs.TextBlock:
TextBlock t = block.Specialise <TextBlock>();
jsonInfo.name += "\t" + t.Text + "\t" + t.TextBlockId;
break;
case BlockIDs.ConsoleBlock:
ConsoleBlock c = block.Specialise <ConsoleBlock>();
jsonInfo.name += "\t" + c.Command + "\t" + c.Arg1 + "\t" + c.Arg2 + "\t" + c.Arg3;
break;
case BlockIDs.DampedSpring:
DampedSpring d = block.Specialise <DampedSpring>();
jsonInfo.name += "\t" + d.Stiffness + "\t" + d.Damping;
break;
case BlockIDs.ServoAxle:
case BlockIDs.ServoHinge:
case BlockIDs.PneumaticAxle:
case BlockIDs.PneumaticHinge:
Servo s = block.Specialise <Servo>();
jsonInfo.name += "\t" + s.MinimumAngle + "\t" + s.MaximumAngle + "\t" + s.MaximumForce + "\t" +
s.Reverse;
break;
case BlockIDs.MotorM:
case BlockIDs.MotorS:
Motor m = block.Specialise <Motor>();
jsonInfo.name += "\t" + m.TopSpeed + "\t" + m.Torque + "\t" + m.Reverse;
break;
default: break;
}
return(jsonInfo);
}
public Arm(IMicrocontroller mcu)
{
grip = new Servo(mcu, 7, 700, 2000);
hand = new Servo(mcu, 6, 500, 2500);
wrist = new Servo(mcu, 8, 500, 2400);
elbow = new Servo(mcu, 5, 700, 2100);
shoulder = new Servo(mcu, 9, 800, 2300);
mount = new Servo(mcu, 10, 1000, 2000);
}
public MeadowApp()
{
Console.WriteLine("Initializing...");
servo = new Servo(Device.CreatePwmPort(Device.Pins.D04), NamedServoConfigs.SG90);
servo.RotateTo(0);
TestServo();
}
/// <summary>
/// place new servo at mouse location
/// </summary>
/// <param name="clickLoc"></param>
private void CreateNewServo(Vector2 clickLoc)
{
Servo newServo = new Servo(clickLoc, _roombaRadius, _pendingServoID);
_servos.Add(newServo);
_entities.Add(newServo);
_activeServoIndex = _servos.Count - 1;
_activeSelected = false;
}
private Servo GetServo(string id)
{
Servo servo = null;
if (!_servos.TryGetValue(id, out servo))
{
throw new Exception(string.Format("Brak serwomechanizmu o identyfikatorze '{0}'.", id));
}
return(servo);
}
public PhidgetServo(PhidgetServoConfiguration config)
{
if (config == null)
throw new ArgumentNullException("config");
configuration = config;
ParseConfiguration();
queue = new Utility.UpdateQueue<Dictionary<Devices, int>>();
servo_controller = new Servo();
}
static void ServoInitialization()
{
servo = new Servo();
servo.Attach += new AttachEventHandler(servo_Attach);
servo.Detach += new DetachEventHandler(servo_Detach);
servo.Error += new ErrorEventHandler(servo_Error);
servo.PositionChange += new PositionChangeEventHandler(
servo_PositionChange);
servo.open();
Console.WriteLine("Waiting for Servo to be attached...");
servo.waitForAttachment();
servo.servos[0].Position = 110.00;
}
public RobotKit()
{
sbt = new Sabertooth2x5();
sbt.RaiseDesignerCreatedEvent();
sbt.SerialPort.Open();
//Fixing some errors
sbt.Motor1.Speed.Maximum = 100;
sbt.Motor1.Speed.Minimum = 0;
sbt.Motor1 = sbt.Motor1;
sbt.Motor2.Speed.Maximum = 100;
sbt.Motor2.Speed.Minimum = 0;
sbt.Motor2 = sbt.Motor2;
servos = new Servo();
servos.open();
System.Threading.Thread.Sleep(200);
try
{
servos.servos[0].Engaged = true;
servos.servos[0].Position = xPos;
}
catch (Exception)
{
//Console.WriteLine("position value out of bounds!");
}
try
{
servos.servos[3].Engaged = true;
servos.servos[3].Position = yPos;
}
catch (Exception)
{
//Console.WriteLine("position value out of bounds!");
}
//sbt.InitialiseSimulation(sbt);
}
public PhidgetServoModule(Servo servo) : base(servo)
{
const double InitPosition = 10.00;
//const double InitMinimumPulseWidth = 600.00;
//const double InitMaximumPulseWidth = 2000.00;
//const double InitDegrees = 120.00;
const bool InitEngaged = true;
// We have only one motor
this.ServoNumber = 0;
this.Count = servo.servos.Count;
this.Type = new string[servo.servos.Count];
this.Position = new double[servo.servos.Count];
//this.Degrees = new double[servo.servos.Count];
//this.MinimumPulseWidth = new double[servo.servos.Count];
//this.MaximumPulseWidth = new double[servo.servos.Count];
this.PositionMin = new double[servo.servos.Count];
this.PositionMax = new double[servo.servos.Count];
this.Engaged = new bool[servo.servos.Count];
//Set the default servo type to the one Phidgets sells
foreach (ServoServo motor in servo.servos)
motor.Type = ServoServo.ServoType.HITEC_HS322HD;
for (var i = 0; i < servo.servos.Count; i++)
{
this.Type[i] = servo.servos[i].Type.ToString();
this.Position[i] = servo.servos[i].Position = InitPosition;
//this.Degrees[i] = InitDegrees;
//this.MinimumPulseWidth[i] = InitMinimumPulseWidth;
//this.MaximumPulseWidth[i] = InitMaximumPulseWidth;
this.PositionMin[i] = servo.servos[i].PositionMin;
this.PositionMax[i] = servo.servos[i].PositionMax;
this.Engaged[i] = InitEngaged;
}
//Setting custom servo parameters example - 600us-2000us == 120 degrees
//servo.servos[this.ServoNumber].setServoParameters(InitMinimumPulseWidth, InitMaximumPulseWidth, InitDegrees);
}
/// <summary>
/// Initialize servo
/// </summary>
private void initServo()
{
servo = new Servo();
servo.Attach += new AttachEventHandler(servo_Attach);
servo.Detach += new DetachEventHandler(servo_Detach);
servo.Error += new ErrorEventHandler(servo_Error);
servo.PositionChange += new PositionChangeEventHandler(servo_PositionChange);
servo.open();
Console.WriteLine("Awaiting Servo attachment..");
servo.waitForAttachment();
}
//When the application is terminating, close the Phidgets.
private void Form1_FormClosed(object sender, FormClosedEventArgs e)
{
servo1.Attach -= new AttachEventHandler(servo_Attach);
servo1.Detach -= new DetachEventHandler(servo_Detach);
servo1.PositionChange -= new PositionChangeEventHandler(servo_PositionChange);
servo2.Attach -= new AttachEventHandler(servo_Attach);
servo2.Detach -= new DetachEventHandler(servo_Detach);
servo2.PositionChange -= new PositionChangeEventHandler(servo_PositionChange);
Application.DoEvents();
servo1.close();
servo1 = null;
servo2.close();
servo2 = null;
}
private void ServoInitialization()
{
servo = new Servo();
servo.Attach += new AttachEventHandler(delegate(object sender, AttachEventArgs e){
Console.WriteLine("Servo {0} attached!", e.Device.SerialNumber.ToString());
});
servo.Detach += new DetachEventHandler(delegate (object sender, DetachEventArgs e){
Console.WriteLine("Servo {0} detached!", e.Device.SerialNumber.ToString());
});
servo.Error += new Phidgets.Events.ErrorEventHandler(delegate(object sender, ErrorEventArgs e){
Console.WriteLine(e.Description);
});
servo.open();
Console.WriteLine("Waiting for Servo to be attached...");
servo.waitForAttachment();
if(servo.Attached)
servo.servos[0].Position = servoInitPos;
}
/// <summary>
/// Waits for input from keyboard to exit.
/// </summary>
private void keepAlive()
{
Console.WriteLine("feed: can goes bananas.");
Console.WriteLine("full: toggle 'full' status.");
Console.WriteLine("exit: exit program.");
String input = Console.ReadLine();
while (!input.Equals("exit"))
{
if (input.Equals("feed"))
{
feedMe();
}
else if (input.Equals("full"))
{
toggleFull();
}
input = Console.ReadLine();
}
Thread.Sleep(SLEEP_TIME_MS);
closeCanOnExit();
Thread.Sleep(SLEEP_TIME_MS);
servo.close();
ifKit.close();
ifKit = null;
servo = null;
}
public void servoInitPhidgetHardware()
{
try
{
//Declare a Servo object
_servo = new Servo();
//Hook the basic event handlers
_servo.Attach += new AttachEventHandler(servo_Attach);
_servo.Detach += new DetachEventHandler(servo_Detach);
_servo.Error += new Phidgets.Events.ErrorEventHandler(servo_Error);
//hook the phidget specific event handlers
_servo.PositionChange += new PositionChangeEventHandler(servo_PositionChange);
//open the Servo object for device connections
_servo.open();
//Get the program to wait for a Servo to be attached
Console.WriteLine("Waiting for Servo to be attached...");
_servo.waitForAttachment();
}
catch (PhidgetException ex)
{
Console.WriteLine(ex.Description);
}
}
public void servoClosePhidgetHardware()
{
_servo.Attach -= new AttachEventHandler(servo_Attach);
_servo.Detach -= new DetachEventHandler(servo_Detach);
_servo.PositionChange -= new PositionChangeEventHandler(servo_PositionChange);
_servo.Error -= new Phidgets.Events.ErrorEventHandler(servo_Error);
//Servo object
_servo.close();
//set the object to null to get it out of memory
_servo = null;
}
//Initialization code
private void Form1_Load(object sender, EventArgs e)
{
servo1 = new Servo();
servo2 = new Servo();
controllerCmb.Enabled = false;
servoNumCombo.Enabled = false;
positionTrk.SetRange(-23, 232);
servo1.Attach += new AttachEventHandler(servo_Attach);
servo1.Detach += new DetachEventHandler(servo_Detach);
servo1.Error += new ErrorEventHandler(servo_Error);
servo1.PositionChange += new PositionChangeEventHandler(servo_PositionChange);
servo2.Attach += new AttachEventHandler(servo_Attach);
servo2.Detach += new DetachEventHandler(servo_Detach);
servo2.Error += new ErrorEventHandler(servo_Error);
servo2.PositionChange += new PositionChangeEventHandler(servo_PositionChange);
servo1.open(serialNum1);
servo2.open(serialNum2);
}
static void ServoStop()
{
servo.close();
servo = null;
Console.WriteLine("ok");
}
