public ServoMotorExample()
{
//This parameters works in a TowerPro MicroServo gg
int milliseconds = 200;
var servo = new ServoMotor(Connectors.GPIO18);
servo.PwmCreate(milliseconds);
for (int i = 13; i < 30; i += 2)
{
Console.WriteLine(i);
servo.DutyCycle = i;
Thread.Sleep(500);
}
for (int i = 30; i >= 13; i -= 2)
{
Console.WriteLine(i);
servo.DutyCycle = i;
Thread.Sleep(500);
}
//servo.DutyCycle = 35;
//while (true) {
// for (int i = 0; i < 100; i+=5) {
Console.WriteLine("End");
// }
//}
//SetPercentage(servo, 0);
//SetPercentage(servo, 100);
//SetPercentage(servo, 50);
//SetPercentage(servo, 0);
}
static void Main(string[] args)
{
Console.WriteLine("Hello Servo Motor!");
using PwmChannel pwmChannel = PwmChannel.Create(0, 0, 50);
using ServoMotor servoMotor = new ServoMotor(pwmChannel, 160, 700, 2200);
WritePulseWidth(pwmChannel, servoMotor);
}
private static void WriteAngle(PwmChannel pwmChannel, ServoMotor servoMotor)
{
servoMotor.Start();
while (true)
{
Console.WriteLine("Enter an angle ('Q' to quit). ");
string angle = Console.ReadLine();
if (angle.ToUpper() == "Q")
{
break;
}
if (!int.TryParse(angle, out int angleValue))
{
Console.WriteLine($"Can not parse {angle}. Try again.");
}
servoMotor.WriteAngle(angleValue);
Console.WriteLine($"Duty Cycle Percentage: {pwmChannel.DutyCyclePercentage}");
}
servoMotor.Stop();
}
static void WritePulseWidth(PwmChannel pwmChannel, ServoMotor servoMotor)
{
servoMotor.Start();
int pulseWidthValue = 1000;
while (true)
{
//Console.WriteLine("Enter a pulse width in microseconds ('Q' to quit). ");
//string? pulseWidth = Console.ReadLine();
//if (pulseWidth?.ToUpper() is "Q" || pulseWidth?.ToUpper() is null)
//{
// break;
//}
//if (!int.TryParse(pulseWidth, out int pulseWidthValue))
//{
// Console.WriteLine($"Can not parse {pulseWidth}. Try again.");
//}
servoMotor.WritePulseWidth(pulseWidthValue);
Console.WriteLine($"Duty Cycle: {pwmChannel.DutyCycle * 100.0}%");
pulseWidthValue = (pulseWidthValue == 1000) ? 2000 : 1000;
Thread.Sleep(5000);
}
servoMotor.Stop();
}
static void WriteAngle(PwmChannel pwmChannel, ServoMotor servoMotor)
{
servoMotor.Start();
while (true)
{
Console.WriteLine("Enter an angle ('Q' to quit). ");
string?angle = Console.ReadLine();
if (angle?.ToUpper() is "Q" || angle?.ToUpper() is null)
{
break;
}
if (!int.TryParse(angle, out int angleValue))
{
Console.WriteLine($"Can not parse {angle}. Try again.");
}
servoMotor.WriteAngle(angleValue);
Console.WriteLine($"Duty Cycle: {pwmChannel.DutyCycle * 100.0}%");
}
servoMotor.Stop();
}
private static void WritePulseWidth(PwmChannel pwmChannel, ServoMotor servoMotor)
{
servoMotor.Start();
while (true)
{
Console.WriteLine("Enter a pulse width in microseconds ('Q' to quit). ");
string pulseWidth = Console.ReadLine();
if (pulseWidth.ToUpper() == "Q")
{
break;
}
if (!int.TryParse(pulseWidth, out int pulseWidthValue))
{
Console.WriteLine($"Can not parse {pulseWidth}. Try again.");
}
servoMotor.WritePulseWidth(pulseWidthValue);
Console.WriteLine($"Duty Cycle: {pwmChannel.DutyCycle * 100.0}%");
}
servoMotor.Stop();
}
static void Main(string[] args)
{
Console.WriteLine("Hello Servo!");
// example of software PWM piloted Servo on GPIO 21
ServoMotor servo = new ServoMotor(21, -1, new ServoMotorDefinition(540, 2470, 20000, 100));
// example of hardware PWM piloted Servo on chip 0 channel 0
// ServoMotor servo = new ServoMotor(0, 0, new ServoMotorDefinition(540, 2470));
if (servo.IsRunningHardwarePwm)
{
Console.WriteLine("We are running on hardware PWM");
}
else
{
Console.WriteLine("We are running on software PWM");
}
while (true)
{
servo.Angle = 0;
Thread.Sleep(1000);
servo.Angle = 100;
Thread.Sleep(1000);
}
}
public ServoDriver(Pca9685 pca9685, int channel)
{
_pca9685 = pca9685;
var pwmChannel = _pca9685.CreatePwmChannel(channel);
_servo = new ServoMotor(pwmChannel, 180, 500, 2500);
}
private void AddServoMotor(ServoMotor sm)
{
var table = document.Tables[servoMotorIndex];
AddServoMotorAxis(table, sm.XAxis, 3);
AddServoMotorAxis(table, sm.YAxis, 9);
AddServoMotorAxis(table, sm.ZAxis, 15);
}
/// <summary>
/// Initializes a new instance of the <see cref="StartPageLogic"/> class.
/// </summary>
public StartPageLogic()
{
_mfrc522 = new MFRC522(BoardConfig.MFRC522CmdPin, BoardConfig.MFRC522ResetPin);
_distanceSensor = new UltrasonicDistanceSensor(BoardConfig.UltrasonicSensorTriggerPin, BoardConfig.UltrasonicSensorEchoPin);
_camera = new PhotoCam();
_motor = new ServoMotor(BoardConfig.ServoMotorPin);
_httpClient = new HttpClient();
}
public void Exception_Should_Throw_When_Writing_Invalid_Angle(int maximumAngle, int angle)
{
Mock <PwmChannel> mockPwmChannel = new Mock <PwmChannel>();
ServoMotor servoMotor = new ServoMotor(mockPwmChannel.Object, maximumAngle);
Assert.Throws <ArgumentOutOfRangeException>(() =>
{
servoMotor.WriteAngle(angle);
});
}
public Switch(byte NumberSwitch, uint MinDur, uint MaxDur, uint MinAng, uint MaxAng, uint ServoAngle)
{
mSwitch = new ServoMotor(GPIO_PIN_D5, new ServoMotorDefinition(MinDur, MaxDur, (uint)SerMotorPeriod.DefaultPeriod, ServoAngle, 5000));
mMinAngle = MinAng;
mMaxAngle = MaxAng;
mNumberSwitch = NumberSwitch;
if ((mNumberSwitch <= 0) && (mNumberSwitch > NUMBER_SWITCH_MAX))
{
new Exception("Not correct number of Signals");
}
mSwitchStatus = new bool[mNumberSwitch];
if (mNumberSwitch <= 2)
{
numOutput = 1;
}
else if (mNumberSwitch <= 4)
{
numOutput = 2;
}
else if (mNumberSwitch <= 8)
{
numOutput = 3;
}
else if (mNumberSwitch <= 16)
{
numOutput = 4;
}
else
{
new Exception("Too many Signals");
}
//initialise the outputs based on the number of signals
var gpio = GpioController.GetDefault();
if (gpio == null)
{
Debug.WriteLine("No GPIO detected on your system");
return;
}
//initialize all multiplexer output to Low
mOut = new GpioPin[mPinTable.Length];
for (int i = 0; i < mPinTable.Length; i++)
{
mOut[i] = gpio.OpenPin(mPinTable[i]);
mOut[i].Write(GpioPinValue.Low);
mOut[i].SetDriveMode(GpioPinDriveMode.Output);
}
Debug.Write("All GPIO initialised in Switch");
//initialise all signals to "false"
for (byte i = 0; i < mNumberSwitch; i++)
{
ChangeSwitch(i, false);
}
}
public void Init()
{
int busId = 1;
I2cConnectionSettings settings = new I2cConnectionSettings(busId, _i2cAddress);
_i2cDevice = I2cDevice.Create(settings);
_pca9685 = new Pca9685(_i2cDevice, pwmFrequency: 50);
_servoChannel = _pca9685.CreatePwmChannel(_channelNumber);
_servoMotor = new ServoMotor(_servoChannel, 180, 500, 2500);
}
public ServoMotorExample()
{
//This parameters works in a TowerPro MicroServo gg
int milliseconds = 200, min = 5, max = 19;
var servo = new ServoMotor(Connectors.GPIO18, milliseconds, min, max);
SetPercentage(servo, 0);
SetPercentage(servo, 100);
SetPercentage(servo, 50);
SetPercentage(servo, 0);
}
private void Start()
{
servo = GetComponent <ServoMotor>();
rigidbody = GetComponent <Rigidbody>();
if (!servo || !rigidbody)
{
Debug.Log("Unable to apply friction to " + name + "! Components not found.");
}
if (!frictionProfile)
{
Debug.Log("Unable to apply friction to " + name + "! Friction profile not set.");
}
}
public void Pwm_Channel_Should_Start_And_Stop_When_Starting_And_Stopping_Servo()
{
Mock <PwmChannel> mockPwmChannel = new Mock <PwmChannel>();
ServoMotor servo = new ServoMotor(mockPwmChannel.Object);
servo.Start();
mockPwmChannel.Verify(channel => channel.Start());
mockPwmChannel.VerifyNoOtherCalls();
mockPwmChannel.Reset();
servo.Stop();
mockPwmChannel.Verify(channel => channel.Stop());
mockPwmChannel.VerifyNoOtherCalls();
}
public void Verify_Duty_Cycle_When_Writing_Pulse_Width(
int frequency,
int pulseWithInMicroseconds,
double expectedDutyCycle)
{
Mock <PwmChannel> mockPwmChannel = new Mock <PwmChannel>();
mockPwmChannel.SetupAllProperties();
mockPwmChannel.Object.Frequency = frequency;
ServoMotor servo = new ServoMotor(mockPwmChannel.Object);
servo.WritePulseWidth(pulseWithInMicroseconds);
Assert.Equal(expectedDutyCycle, mockPwmChannel.Object.DutyCycle, 5);
}
public void Init()
{
Servo = new ServoMotor(PwmDevice, PwmChannel.C1, 150, 600);
DcMotor = new DcMotor(PwmDevice, PwmChannel.C4, PwmChannel.C5);
Stepper = new StepperMotor(
PwmDevice
, PwmChannel.C11
, PwmChannel.C10
, PwmChannel.C9
, PwmChannel.C8);
Stepper.RotationCompleted += OnStepperCompleted;
Led0 = new Led(PwmDevice, PwmChannel.C0);
}
static void TestRotate()
{
Console.WriteLine("Testing a servo motor controlled by a rotary encoder");
var angle = 0;
const int step = 10;
using (var left = Pi.Gpio.Pin(leftPin, PinKind.InputPullUp))
using (var right = Pi.Gpio.Pin(rightPin, PinKind.InputPullUp))
using (var servo = new ServoMotor(maxAngle))
{
servo.PulseWidths(0.5, 2.5);
servo.Start();
// Connect rotate and click events
left.OnRisingEdge += Rotate;
Console.WriteLine("Click the button to stop this test");
Pi.Gpio.Pin(buttonPin, PinKind.InputPullUp).WaitForEdge(PinEdge.Rising);
left.OnRisingEdge -= Rotate;
servo.Angle = 0;
Pi.Wait(2000);
servo.Stop();
// Called on the falling edge of the left pin
void Rotate(object sender, PinEventHandlerArgs args)
{
if (args.Bounced)
{
return;
}
var a = angle;
a += left.Value == right.Value ? -step : step;
if (a < 0)
{
a = 0;
}
if (a > maxAngle)
{
a = maxAngle;
}
if (a != angle)
{
Console.WriteLine("servo angle at {0}°", servo.Angle = angle = a);
}
}
}
}
void TickIntellect()
{
//horizontalMotor
{
float throttle = 0;
throttle += Intellect.GetControlKeyStrength(GameControlKeys.Left);
throttle -= Intellect.GetControlKeyStrength(GameControlKeys.Right);
GearedMotor motor = PhysicsModel.GetMotor("horizontalMotor") as GearedMotor;
if (motor != null)
{
motor.Throttle = throttle;
}
}
//gibbetMotor
{
ServoMotor motor = PhysicsModel.GetMotor("gibbetMotor") as ServoMotor;
if (motor != null)
{
Radian needAngle = motor.DesiredAngle;
needAngle += Intellect.GetControlKeyStrength(GameControlKeys.Forward) * .004f;
needAngle -= Intellect.GetControlKeyStrength(GameControlKeys.Backward) * .004f;
MathFunctions.Clamp(ref needAngle,
new Degree(-20.0f).InRadians(), new Degree(40.0f).InRadians());
motor.DesiredAngle = needAngle;
}
}
//Change player LookDirection at rotation
PlayerIntellect intellect = Intellect as PlayerIntellect;
if (intellect != null)
{
Vec3 lookVector = intellect.LookDirection.GetVector();
lookVector *= OldRotation.GetInverse();
lookVector *= Rotation;
intellect.LookDirection = SphereDir.FromVector(lookVector);
}
}
public static void Main()
{
Debug.WriteLine("devMobile.Longboard.ServoTest starting");
try
{
AdcController adc = AdcController.GetDefault();
AdcChannel adcChannel = adc.OpenChannel(0);
#if POSITIONAL
ServoMotor servo = new ServoMotor("TIM5", ServoMotor.ServoType.Positional, PinNumber('A', 0));
servo.ConfigurePulseParameters(0.1, 2.3);
#endif
#if CONTINUOUS
ServoMotor servo = new ServoMotor("TIM5", ServoMotor.ServoType.Continuous, PinNumber('A', 0));
servo.ConfigurePulseParameters(0.1, 2.3);
#endif
while (true)
{
double value = adcChannel.ReadRatio();
#if POSITIONAL
double position = Map(value, 0.0, 1.0, 0.0, 180);
#endif
#if CONTINUOUS
double position = Map(value, 0.0, 1.0, -100.0, 100.0);
#endif
Debug.WriteLine($"Value: {value:F2} Position: {position:F1}");
servo.Set(position);
Thread.Sleep(100);
}
}
catch (Exception ex)
{
Debug.WriteLine(ex.Message);
}
}
public void Verify_Duty_Cycle_When_Writing_Angle(
int frequency,
int maxiumAngle,
int minimumPulseWidthMicroseconds,
int maximumPulseWidthMicroseconds,
int angle,
double expectedDutyCycle)
{
Mock <PwmChannel> mockPwmChannel = new Mock <PwmChannel>();
mockPwmChannel.SetupAllProperties();
mockPwmChannel.Object.Frequency = frequency;
ServoMotor servo = new ServoMotor(
mockPwmChannel.Object,
maxiumAngle,
minimumPulseWidthMicroseconds,
maximumPulseWidthMicroseconds);
servo.WriteAngle(angle);
Assert.Equal(expectedDutyCycle, mockPwmChannel.Object.DutyCycle, 5);
}
public void Init()
{
Servo = new ServoMotor(PwmDevice, PwmChannel.C1, 150, 600);
Led0 = new Led(PwmDevice, PwmChannel.C0);
}
private void TickIntellect()
{
GUIshit();
MapObjectAttachedParticle JetFire1 = GetFirstAttachedObjectByAlias("JetFire1") as MapObjectAttachedParticle;
MapObjectAttachedParticle JetFire2 = GetFirstAttachedObjectByAlias("JetFire2") as MapObjectAttachedParticle;
float speed = GetRealSpeed();
AKunit akunit = GetPlayerUnit() as AKunit;
Vec3 dir = AKVTOLBody.Rotation.GetForward();
Radian slopeAngle = MathFunctions.ATan(dir.Z, dir.ToVec2().Length());
MASS = 0;
foreach (Body body in PhysicsModel.Bodies)
{
MASS += body.Mass;
}
ShiftBooster();
AKVTOLOn = Intellect != null && Intellect.IsActive();
// GUItest();
ALTray();
//engine force + sound pitch control
if (Intellect.IsControlKeyPressed(GameControlKeys.Jump))
{
if (akunit != null)
{
akunit.GunsTryFire(false);
}
}
if (Intellect.IsControlKeyPressed(GameControlKeys.Forward))
{
force += forceadd;
}
else if (Intellect.IsControlKeyPressed(GameControlKeys.Backward))
{
force -= forceadd;
}
else
{
}
if (Intellect.IsControlKeyPressed(GameControlKeys.VerticleTakeOff_L_EngineUp))
{
EngineDir += 2f;
}
else if (Intellect.IsControlKeyPressed(GameControlKeys.VerticleTakeOff_L_EngineDown))
{
EngineDir -= 2f;
}
else
{
}
MathFunctions.Clamp(ref EngineDir, 0, 90);
EngineApp.Instance.ScreenGuiRenderer.AddText("Throttle: " + force, new Vec2(.6f, .1f));
if (JetFire1 != null && JetFire2 != null)
{
if (force > 85f)
{
JetFire1.Visible = true;
JetFire2.Visible = true;
}
else
{
JetFire1.Visible = false;
JetFire2.Visible = false;
}
}
enpitch = 0.8f + (0.6f * (force / 100));
MathFunctions.Clamp(ref force, 0.1f, 100);
MathFunctions.Clamp(ref enpitch, 0.8f, 1.4f);
//update jet channel position and pitch
if (rotorSoundChannel != null)
{
//update channel
rotorSoundChannel.Pitch = enpitch;
rotorSoundChannel.Volume = 1;
rotorSoundChannel.Position = Position;
//rotorSoundChannel.MinDistance = 10;
}
//end of engine force + sound pitch control
//Forces
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//start VTOL Pitch (Y turn)
if (Intellect.IsControlKeyPressed(GameControlKeys.Arrow_Up) || Intellect.IsControlKeyPressed(GameControlKeys.Arrow_Down))
{
float AUp = Intellect.GetControlKeyStrength(GameControlKeys.Arrow_Up) / 2;
float ADown = Intellect.GetControlKeyStrength(GameControlKeys.Arrow_Down) / 2;
Hpitch += (AUp - ADown);
MathFunctions.Clamp(ref Hpitch, -10, 10);
}
else
{
if (Hpitch != 0)
{
Hpitch -= Hpitch / 5;
if ((Hpitch - (Hpitch % 1)) == 0)
{
Hpitch = 0;
}
}
else
{
float mammadpitch = (AKVTOLBody.AngularVelocity * AKVTOLBody.Rotation.GetInverse()).Y * 2;
MathFunctions.Clamp(ref mammadpitch, -10f, 10);
AKVTOLBody.AddForce(ForceType.GlobalAtLocalPos, TickDelta,
AKVTOLBody.Rotation * new Vec3(0, 0, -mammadpitch) * MASS, new Vec3(-8, 0, 0));
EngineApp.Instance.ScreenGuiRenderer.AddText("MammadPitch: " + mammadpitch.ToString(), new Vec2(.1f, .2f));
}
}
if (Hpitch != 0)
{
AKVTOLBody.AddForce(ForceType.GlobalTorque, TickDelta,
AKVTOLBody.Rotation * new Vec3(0, Hpitch, 0) * MASS, Vec3.Zero);
}
//end of VTOL pitch (Y turn)
//start jet Z turn
if (Intellect.IsControlKeyPressed(GameControlKeys.Right) || Intellect.IsControlKeyPressed(GameControlKeys.Left))
{
float right = Intellect.GetControlKeyStrength(GameControlKeys.Right) / 2;
float left = Intellect.GetControlKeyStrength(GameControlKeys.Left) / 2;
TrunZ += (left - right);
MathFunctions.Clamp(ref TrunZ, -10, 10);
AKVTOLBody.AddForce(ForceType.LocalTorque, TickDelta,
new Vec3(0, 0, TrunZ * 2) * MASS, Vec3.Zero);
}
else
{
TrunZ = 0;
}
//end of jet Z turn
//start jet X turn
if (Intellect.IsControlKeyPressed(GameControlKeys.Arrow_Right) || Intellect.IsControlKeyPressed(GameControlKeys.Arrow_Left))
{
float rightX = Intellect.GetControlKeyStrength(GameControlKeys.Arrow_Right) / 2;
float leftX = Intellect.GetControlKeyStrength(GameControlKeys.Arrow_Left) / 2;
TrunX += (rightX - leftX);
MathFunctions.Clamp(ref TrunX, -10, 10);
AKVTOLBody.AddForce(ForceType.GlobalTorque, TickDelta,
AKVTOLBody.Rotation * new Vec3(TrunX * 2, 0, 0) * MASS, Vec3.Zero);
}
else
{
TrunX = 0;
}
float SHOOT = AKVTOLBody.Rotation.GetInverse().ToAngles().Roll; //1 - Rotation.GetUp().Z;
if (SHOOT < 0)
{
SHOOT = -SHOOT;
}
if (SHOOT > 90)
{
SHOOT = 90 - (SHOOT - 90);
}
AKVTOLBody.AddForce(ForceType.GlobalAtLocalPos, TickDelta,
AKVTOLBody.Rotation * new Vec3(0, 0, -SHOOT / 180) * MASS, new Vec3(-8, 0, 0));
//end of jet X turn
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//adding anty Y movment force
float Yshit = (AKVTOLBody.LinearVelocity * Rotation.GetInverse()).Y;
EngineApp.Instance.ScreenGuiRenderer.AddText("Yshit: " + Yshit.ToString(), new Vec2(.6f, .3f));
EngineApp.Instance.ScreenGuiRenderer.AddText("roll: " + Rotation.ToAngles().Roll, new Vec2(.6f, .35f));
EngineApp.Instance.ScreenGuiRenderer.AddText("roll: " + (Rotation.GetInverse().ToAngles()).ToString(), new Vec2(.6f, .4f));
EngineApp.Instance.ScreenGuiRenderer.AddText("Edir: " + EngineDir.ToString(), new Vec2(.1f, .6f));
//start of adding force
MathFunctions.Clamp(ref force, 0.1f, 100);
EngineApp.Instance.ScreenGuiRenderer.AddText("speed: " + GetRealSpeed().ToString(), new Vec2(.6f, .15f));
GUItest();
//anti gravity when jet have speed (wings force)
float antyshityforcy = GetRealSpeed() / 10;
float slopeangleshit = 1.5f;
MathFunctions.Clamp(ref antyshityforcy, 0, 10);
if (slopeAngle > 0)
{
slopeangleshit = 1.5f - slopeAngle;
}
else
{
slopeangleshit = 0.5f;
}
if (GetRealSpeed() > 0)
{
AKVTOLBody.AddForce(ForceType.LocalAtLocalPos, TickDelta,
new Vec3(0, 0, antyshityforcy * slopeangleshit) * MASS, Vec3.Zero);
}
//if Max Alt is not reached add jet motor force
if (AKVTOLBody.Position.Z < Type.MaxAlt)
{
AKVTOLBody.AddForce(ForceType.GlobalAtLocalPos, TickDelta,
AKVTOLBody.Rotation * new Vec3(force / 2f * ((90 - EngineDir) / 90), 0, force / 8f * (EngineDir / 90)) * MASS, Vec3.Zero);
}
//dampings
AKVTOLBody.AngularDamping = 2f + (speed / 60);
AKVTOLBody.LinearDamping = 0.6f;
ServoMotor Lenginem = PhysicsModel.GetMotor("LEngineM") as ServoMotor;
ServoMotor Renginem = PhysicsModel.GetMotor("REngineM") as ServoMotor;
if (Lenginem != null && Renginem != null)
{
float EngingDirRad = EngineDir * MathFunctions.PI / 180;
Renginem.DesiredAngle = EngingDirRad;
Lenginem.DesiredAngle = EngingDirRad;
}
}
static void MoveToAngle(ServoMotor Servo, int Angle)
{
Servo.WriteAngle(Angle);
}
static void Main()
{
TouchSensor touch = new TouchSensor(FEZ.GpioPin.D7);
LightSensor light = new LightSensor(FEZ.AdcChannel.A3);
ServoMotor servo = new ServoMotor(FEZ.PwmPin.Controller4.Id, FEZ.PwmPin.Controller4.D5);
Buzzer buzz = new Buzzer(FEZ.GpioPin.D4);
LcdRgbBacklight lcd = new LcdRgbBacklight();
host = "192.168.1.152";
port = 80;
var buffer = new byte[512];
var cont = GpioController.GetDefault();
var reset = cont.OpenPin(FEZ.GpioPin.WiFiReset);
var irq = cont.OpenPin(FEZ.GpioPin.WiFiInterrupt);
var spi = SpiDevice.FromId(FEZ.SpiBus.WiFi, SPWF04SxInterface.GetConnectionSettings(FEZ.GpioPin.WiFiChipSelect));
connected = false;
socketOpened = false;
garageLed = cont.OpenPin(FEZ.GpioPin.D2);
servo.SetPosition(180);
garageLed.SetDriveMode(GpioPinDriveMode.Output);
wifi = new SPWF04SxInterface(spi, irq, reset);
wifi.IndicationReceived += (s, e) => Debug.WriteLine($"WIND: {Program.WindToName(e.Indication)} {e.Message}");
wifi.ErrorReceived += (s, e) => Debug.WriteLine($"ERROR: {e.Error} {e.Message}");
wifi.TurnOn();
//wifi.JoinNetwork("GHI", "ghi555wifi.");
lcd.Clear();
lcd.SetBacklightRGB(100, 100, 100);
lcd.Write("Time:");
while (!connected)
{
ListenWind();
Thread.Sleep(200);
}
StringBuilder builder = new StringBuilder();
while (connected)
{
if (!socketOpened)
{
id = wifi.OpenSocket(host, port, SPWF04SxConnectionyType.Tcp, SPWF04SxConnectionSecurityType.None);
socketOpened = true;
}
var hour = DateTime.UtcNow.Hour;
var minute = DateTime.UtcNow.Minute;
var second = DateTime.UtcNow.Second;
lcd.SetCursor(7, 1);
lcd.Write($"{hour}:{minute}:{second}");
if (touch.IsTouched())
{
wifi.WriteSocket(id, Encoding.UTF8.GetBytes("Someone wants to open the garage"));
}
if (light.ReadLightLevel() > 60 && isDoorOpened == true)
{
//Debug.WriteLine(light.ReadLightLevel().ToString());
wifi.WriteSocket(id, Encoding.UTF8.GetBytes("Car in the garage"));
while (light.ReadLightLevel() > 60)
{
Thread.Sleep(50);
}
wifi.WriteSocket(id, Encoding.UTF8.GetBytes("You can close the garage"));
}
if (wifi.QuerySocket(id) is var avail && avail > 0)
{
wifi.ReadSocket(id, buffer, 0, Math.Min(avail, buffer.Length));
for (var k = 0; k < buffer.Length; k++)
{
if (buffer[k] != 0)
{
char result = (char)buffer[k];
builder.Append(result);
buffer[k] = 0;
}
}
Debug.WriteLine(builder.ToString());
}
string command = builder.ToString();
builder.Clear();
switch (command)
{
case "open":
buzz.Beep();
servo.SetPosition(0);
garageLed.Write(GpioPinValue.High);
isDoorOpened = true;
break;
case "close":
buzz.Beep();
servo.SetPosition(180);
garageLed.Write(GpioPinValue.Low);
break;
default:
break;
}
Thread.Sleep(100);
}
}
public PanTiltController(ServoMotor panServo, ServoMotor tiltServo)
{
_panServo = panServo;
_tiltServo = tiltServo;
}
/// <summary>
/// Initializes a new instance of the
/// <see cref="StudentPiER.StudentCode"/> class.
/// </summary>
/// <param name='robot'>
/// The Robot to associate with this StudentCode
/// </param>
public StudentCode(Robot robot)
{
Debug.Print("hello");
this.robot = robot;
this.stopwatch = new Stopwatch();
this.stopwatch.Start();
this.useRfid = true;
if (this.useRfid)
{
this.rfid = new Rfid(robot);
}
this.leftMotor = new GrizzlyBear(robot, Watson.Motor.M0);
this.rightMotor = new GrizzlyBear(robot, Watson.Motor.M1);
this.gearbox = new GrizzlyBear(robot, Watson.Motor.M2, 0, 100, true);
this.sonar = new AnalogSonarDistanceSensor(robot, Watson.Analog.A5);
this.doorController = new MicroMaestro(robot, 12);
this.motorDoor = new ServoMotor(robot, doorController,0,2400,9600);
this.leftEncoder = new GrizzlyEncoder(10, leftMotor, robot);
this.rightEncoder = new GrizzlyEncoder(10, rightMotor, robot);
}
static async Task Main(string[] args)
{
Console.WriteLine("Hello World!");
using PwmChannel pwmChannel1 = PwmChannel.Create(0, 0, 50);
using ServoMotor servoMotor1 = new ServoMotor(
pwmChannel1,
180,
700,
2400);
using PwmChannel pwmChannel2 = PwmChannel.Create(0, 1, 50);
using ServoMotor servoMotor2 = new ServoMotor(
pwmChannel2,
180,
700,
2400);
using SoftwarePwmChannel pwmChannel3 = new SoftwarePwmChannel(27, 50, 0.5);
using ServoMotor servoMotor3 = new ServoMotor(
pwmChannel3,
180,
900,
2100);
servoMotor1.Start();
servoMotor2.Start();
servoMotor3.Start();
connection = new HubConnectionBuilder()
.WithUrl("https://192.168.1.162:5001/chathub", conf =>
{
conf.HttpMessageHandlerFactory = (x) => new HttpClientHandler
{
ServerCertificateCustomValidationCallback = HttpClientHandler.DangerousAcceptAnyServerCertificateValidator,
};
})
.Build();
try
{
connection.On <string, string>("ReceiveMessage", (user, message) =>
{
if (user == "servo1")
{
MoveToAngle(servoMotor1, Int32.Parse(message));
}
else if (user == "servo2")
{
MoveToAngle(servoMotor2, Int32.Parse(message));
}
else if (user == "servo3")
{
MoveToAngle(servoMotor3, Int32.Parse(message));
}
Console.WriteLine($"{message} posted by: {user}");
});
await connection.StartAsync();
}
catch (System.Exception)
{
throw;
}
//
// Keep the code running...
//
while (true)
{
}
servoMotor1.Stop();
servoMotor2.Stop();
servoMotor3.Stop();
}
private void TickChassis()
{
bool onGround = leftTrack.onGround || rightTrack.onGround;
OnGround = onGround;
float leftTrackThrottle = 0;
float rightTrackThrottle = 0;
if (Intellect != null)
{
Reset(false);
Wheels();
ShiftBooster();
GearchangeDtime();
//force for stability
float speedkmph = GetRealSpeed() * 3;
float speedpure = speedkmph - (speedkmph % 1);
MapObjectAttachedBillboard ta1 = GetFirstAttachedObjectByAlias("tail1")
as MapObjectAttachedBillboard;
MapObjectAttachedBillboard ta2 = GetFirstAttachedObjectByAlias("tail2")
as MapObjectAttachedBillboard;
MapObjectAttachedBillboard ta3 = GetFirstAttachedObjectByAlias("tail3")
as MapObjectAttachedBillboard;
MapObjectAttachedBillboard ta4 = GetFirstAttachedObjectByAlias("tail4")
as MapObjectAttachedBillboard;
bool backlightred = false;
bool backlightw = false;
{
ServoMotor THREF = PhysicsModel.GetMotor("FB") as ServoMotor;
Radian FB = 0;
float forward = Intellect.GetControlKeyStrength(GameControlKeys.Forward);
leftTrackThrottle += forward;
rightTrackThrottle += forward;
float backward = Intellect.GetControlKeyStrength(GameControlKeys.Backward);
leftTrackThrottle -= backward;
rightTrackThrottle -= backward;
if (Intellect.IsControlKeyPressed(GameControlKeys.Forward))
{
if (GetRealSpeed() < 0.01)
{
backlightred = true;
}
FB++;
}
else if (Intellect.IsControlKeyPressed(GameControlKeys.Backward))
{
FB--;
if (GetRealSpeed() > 0.5)
{
backlightred = true;
}
if (GetRealSpeed() < 0.01)
{
backlightw = true;
}
}
MathFunctions.Clamp(ref FB,
new Degree(-1.0f).InRadians(), new Degree(1.0f).InRadians());
THREF.DesiredAngle = FB;
if (ta1 != null)
{
ta1.Visible = backlightred;
ta2.Visible = backlightred;
ta3.Visible = backlightw;
ta4.Visible = backlightw;
}
}
{
ServoMotor wmotor = PhysicsModel.GetMotor("wheel") as ServoMotor;
ServoMotor wmotor_2 = PhysicsModel.GetMotor("wheel2") as ServoMotor;
Radian needAngle = wmotor.DesiredAngle;
float left = Intellect.GetControlKeyStrength(GameControlKeys.Left);
float right = Intellect.GetControlKeyStrength(GameControlKeys.Right);
if (left > 0)
{
needAngle -= 0.06f;
}
else if (right > 0)
{
needAngle += 0.06f;
}
else
{
needAngle = 0f;
}
float TBaseForce = 0;
float Pspeed = GetRealSpeed();
if (Pspeed < 0)
{
Pspeed = -Pspeed;
}
TBaseForce = left + (-right);
float speedcoef = 1;
if (GetRealSpeed() < 10 && GetRealSpeed() > -10)
{
speedcoef = GetRealSpeed() / 10;
}
if (speedcoef < 0)
{
speedcoef = -speedcoef;
}
if (!Intellect.IsControlKeyPressed(GameControlKeys.Forward) && !Intellect.IsControlKeyPressed(GameControlKeys.Backward) && speedcoef != 1)
{
TBaseForce = TBaseForce * 1.5f;
}
if (GetRealSpeed() < 0)
{
TBaseForce = -TBaseForce;
}
float SpeedD = 120 / GetRealSpeed();
MathFunctions.Clamp(ref SpeedD, 1, 1.6f);
float TMainForce = TBaseForce * chassisBody.Mass * SpeedD * 10;
if (OnGround)
{
chassisBody.AddForce(ForceType.LocalTorque, TickDelta,
new Vec3(0, 0, TMainForce * speedcoef * 2), Vec3.Zero);
}
MathFunctions.Clamp(ref needAngle,
new Degree(-29.0f).InRadians(), new Degree(29.0f).InRadians());
if (wmotor != null)
{
wmotor.DesiredAngle = needAngle;
wmotor_2.DesiredAngle = needAngle;
}
}
}
Vec3 localLinearVelocity = chassisBody.LinearVelocity * chassisBody.Rotation.GetInverse();
//add drive force
float slopeForwardForceCoeffient;
float slopeBackwardForceCoeffient;
float slopeLinearDampingAddition;
{
Vec3 dir = chassisBody.Rotation.GetForward();
Radian slopeAngle = MathFunctions.ATan(dir.Z, dir.ToVec2().Length());
Radian maxAngle = MathFunctions.PI / 4;//new Degree(45)
slopeForwardForceCoeffient = 1;
if (slopeAngle > maxAngle)
{
slopeForwardForceCoeffient = 0;
}
slopeBackwardForceCoeffient = 1;
if (slopeAngle < -maxAngle)
{
slopeBackwardForceCoeffient = 0;
}
MathFunctions.Clamp(ref slopeForwardForceCoeffient, 0, 1);
MathFunctions.Clamp(ref slopeBackwardForceCoeffient, 0, 1);
slopeLinearDampingAddition = localLinearVelocity.X > 0 ? slopeAngle : -slopeAngle;
//slopeLinearDampingAddition *= 1;
if (slopeLinearDampingAddition < 0)
{
slopeLinearDampingAddition = 0;
}
}
if (leftTrack.onGround)
{
if (leftTrackThrottle > 0 && localLinearVelocity.X < Type.MaxForwardSpeed)
{
float force = localLinearVelocity.X > 0 ? currentGear.GearDriveForwardForce + NOSBoost + DownPower : currentGear.GearBrakeForce;
force *= leftTrackThrottle;
force *= slopeForwardForceCoeffient;
chassisBody.AddForce(ForceType.LocalAtLocalPos, TickDelta,
new Vec3(force, 0, 0), new Vec3(0, tracksPositionYOffset, 0));
}
if (leftTrackThrottle < 0 && (-localLinearVelocity.X) < Type.MaxBackwardSpeed)
{
float force = currentGear.GearBrakeForce; //: Type.DriveBackwardForce;
force *= leftTrackThrottle;
force *= slopeBackwardForceCoeffient;
chassisBody.AddForce(ForceType.LocalAtLocalPos, TickDelta,
new Vec3(force, 0, 0), new Vec3(0, tracksPositionYOffset, 0));
}
}
if (rightTrack.onGround)
{
if (rightTrackThrottle > 0 && localLinearVelocity.X < Type.MaxForwardSpeed)
{
float force = localLinearVelocity.X > 0 ? currentGear.GearDriveForwardForce + NOSBoost + DownPower : currentGear.GearBrakeForce;
force *= rightTrackThrottle;
force *= slopeForwardForceCoeffient;
chassisBody.AddForce(ForceType.LocalAtLocalPos, TickDelta,
new Vec3(force, 0, 0), new Vec3(0, -tracksPositionYOffset, 0));
}
if (rightTrackThrottle < 0 && (-localLinearVelocity.X) < Type.MaxBackwardSpeed)
{
float force = currentGear.GearBrakeForce; //: Type.DriveBackwardForce;
force *= rightTrackThrottle;
force *= slopeBackwardForceCoeffient;
chassisBody.AddForce(ForceType.LocalAtLocalPos, TickDelta,
new Vec3(force, 0, 0), new Vec3(0, -tracksPositionYOffset, 0));
}
}
//LinearVelocity
if (onGround && localLinearVelocity != Vec3.Zero)
{
Vec3 velocity = localLinearVelocity;
velocity.Y = 0;
chassisBody.LinearVelocity = chassisBody.Rotation * velocity;
}
bool stop = false; // onGround && leftTrackThrottle == 0 && rightTrackThrottle == 0;
bool noLinearVelocity = chassisBody.LinearVelocity.Equals(Vec3.Zero, .2f);
bool noAngularVelocity = chassisBody.AngularVelocity.Equals(Vec3.Zero, .2f);
//AngularDamping
if (onGround)
{
//LinearDamping
float linearDamping;
if (stop)
{
linearDamping = noLinearVelocity ? 1 : 1;
}
else
{
linearDamping = .15f;
}
chassisBody.LinearDamping = linearDamping + slopeLinearDampingAddition;
if (stop && noAngularVelocity)
{
chassisBody.AngularDamping = 5;
}
else
{
chassisBody.AngularDamping = 1;
}
}
else
{
chassisBody.AngularDamping = 0.55f;
chassisBody.LinearDamping = 0.05f;
}
}
//for hopper delay
//private Boolean dump;
//private Boolean notDump;
//For stage 3 autonomous
//private int n;
/// <summary>
/// Initializes a new instance of the
/// <see cref="StudentPiER.StudentCode"/> class.
/// </summary>
/// <param name='robot'>
/// The Robot to associate with this StudentCode
/// </param>
public StudentCode(Robot robot)
{
this.robot = robot;
this.stopwatch = new Stopwatch();
this.stopwatch.Start();
this.useRfid = false;
/*if (this.useRfid)
{
this.rfid = new Rfid(robot);
}*/
this.leftMotor = new GrizzlyBear(robot, Watson.Motor.M0);
this.rightMotor = new GrizzlyBear(robot, Watson.Motor.M1);
this.conveyorBeltMotor = new GrizzlyBear(robot, Watson.Motor.M3);
this.hopperMotor = new GrizzlyBear(robot, Watson.Motor.M2);
this.sonar = new AnalogSonarDistanceSensor(robot, Watson.Analog.A5);
this.leftEncoder = new GrizzlyEncoder(Step, this.leftMotor, this.robot);
this.rightEncoder = new GrizzlyEncoder(Step, this.rightMotor, this.robot);
this.servoSwitch = new DigitalLimitSwitch(robot, Watson.Digital.D1);
this.autonomousSwitch = new DigitalLimitSwitch(robot, Watson.Digital.D3);
this.StartTime = true;
this.mm = new MicroMaestro(robot, 12);
this.servo0 = new ServoMotor(robot, mm, 0, 500, 2000);
//this.dump = true;
//this.notDump = false;
//this.n = 0;
}
public PanTiltMechanism(IPwmDevice pwmDevice) : base(pwmDevice)
{
PanServo = new ServoMotor(PwmDevice, PwmChannel.C1, 130, 670);
TiltServo = new ServoMotor(PwmDevice, PwmChannel.C0, 130, 670);
}
