public PositionPID(MotorPort port, Int32 position, bool brake, sbyte maxPower, float P, float I, float D, float sampleTime)
: base(P,I,D,sampleTime, (float) maxPower, -((float) maxPower))
{
this.motor = new Motor(port);
target = position;
this.brake = brake;
}
public PositionPID (Motor motor, Int32 position, bool brake, sbyte maxPower, float P, float I, float D, int settleTimeMs):
base(P,I,D,SampleTime, (float) maxPower, -((float) maxPower))
{
movingAverage = new MovingAverage((uint)((uint)settleTimeMs/(uint)SampleTime));
this.motor = motor;
target = position;
this.brake = brake;
}
public static void Main(string[] args)
{
var sensor = new EV3ColorSensor(SensorPort.In1);
sensor.Mode = ColorMode.RGB;
Motor motorA = new Motor(MotorPort.OutA);
Motor motorB = new Motor(MotorPort.OutB);
Motor motorD = new Motor(MotorPort.OutD);
string solution;
Cube cube = new Cube (motorA, motorB, motorD); // instantiate a real cube
// Cube cube = new Cube (); // virtual cube used for tests
// Solver.Randomizer (cube, 5000); // scramble the virtual cube
DateTime starttime = DateTime.Now;
TimeSpan elapsedtime = new TimeSpan ();
MoveCube.BuildCube2P(cube, MoveCube.BuildCube (cube, motorA, motorB, motorD, sensor));
elapsedtime = DateTime.Now - starttime;
Console.WriteLine (cube.GetCubeMap());
Console.WriteLine (cube.ToString ());
Console.WriteLine ("Elapsed time scanning cube: {0}", elapsedtime);
starttime = DateTime.Now;
// uncomment line below if you want to use the internal algorithm to solve the cube.
// It's the easiest way, but also the slowest.
// solution = Solver.Solve (cube);
// comment the following block if you intend to use the internal algorithm to solve
// the cube. By default the main program tries to read the solution from a solution
// file. Just run the Kociemba algorithm on a pc and use scp, putty or any other ssh
// tool to send the file to the intelligent Lego brick.
Communication.ClearCube (Communication.CubePath);
Communication.ClearCube (Communication.SolutionPath);
Communication.SaveCube (Communication.CubePath, cube.ToString ());
solution = Communication.ReceiveSolution ();
elapsedtime = DateTime.Now - starttime;
Console.WriteLine ("Solution: {0}", solution);
Console.WriteLine ("Elapsed time receiving solution: {0}", elapsedtime);
starttime = DateTime.Now;
Solver.TranslateMove (solution, cube);
Console.WriteLine (cube.GetCubeMap ());
elapsedtime = DateTime.Now - starttime;
Console.WriteLine ("Time elapsed moving cube: {0}", elapsedtime);
Communication.ClearCube (Communication.CubePath);
Communication.ClearCube (Communication.SolutionPath);
}
public static void Main(string[] args)
{
InfoDialog dialog = new InfoDialog ("Attach a motor to port A", true);
dialog.Show ();//Wait for enter to be pressed
Motor motor = new Motor (MotorPort.OutA);
motor.SetSpeed (50);
Thread.Sleep (3000);
motor.Off ();
Lcd.Instance.Clear ();
}
/// <summary>
/// コンストラクタ
/// </summary>
public Balanc3R()
{
motorR = new Motor(MotorPort.OutA); // 右足
motorL = new Motor(MotorPort.OutD); // 左足
speaker = new Speaker(0);
gyro = new EV3GyroSensor(SensorPort.In2);
// 角速度を取得
gyro.Mode = GyroMode.AngularVelocity;
ir = new EV3IRSensor(SensorPort.In4);
ir.Mode = IRMode.Remote;
}
public static readonly int SensorSide = 87; //79
#endregion Fields
#region Methods
/*
* This method controls the movements of the robot to position
* each face of the cube to be scanned by the color sensor.
*/
public static char[] BuildCube(Cube cube, Motor motorA, Motor motorB, Motor motorD, EV3ColorSensor sensor)
{
char[] faces = new char[6];
// Position face in cube
for (int j = 0; j < 6; j++) {
switch (j) {
case 0:
BuildFace (cube.face[3], motorA, motorB, sensor);
cube.face [3].TurnCWCore ();
cube.face [3].TurnCWCore ();
MoveCube.Move (motorD, MoveCube.GrabArm);
MoveCube.Move (motorD, MoveCube.PullArm);
MoveCube.Move (motorD, MoveCube.RestArm);
break;
case 1:
BuildFace (cube.face[2], motorA, motorB, sensor);
cube.face [2].TurnCWCore ();
MoveCube.Move (motorD, MoveCube.GrabArm);
MoveCube.Move (motorD, MoveCube.PullArm);
MoveCube.Move (motorD, MoveCube.RestArm);
break;
case 2:
BuildFace (cube.face[1], motorA, motorB, sensor);
MoveCube.MoveRel (motorA, MoveCube.CCW90);
MoveCube.Move (motorD, MoveCube.GrabArm);
MoveCube.Move (motorD, MoveCube.PullArm);
MoveCube.Move (motorD, MoveCube.RestArm);
break;
case 3:
BuildFace (cube.face[0], motorA, motorB, sensor);
cube.face [0].TurnCCWCore ();
MoveCube.MoveRel (motorA, MoveCube.CW90);
MoveCube.Move (motorD, MoveCube.GrabArm);
MoveCube.Move (motorD, MoveCube.PullArm);
MoveCube.Move (motorD, MoveCube.RestArm);
break;
case 4:
BuildFace (cube.face [4], motorA, motorB, sensor);
MoveCube.Move (motorD, MoveCube.GrabArm);
MoveCube.Move (motorD, MoveCube.PullArm);
MoveCube.Move (motorD, MoveCube.RestArm);
break;
case 5:
BuildFace (cube.face [5], motorA, motorB, sensor);
break;
}
}
for (int i = 0; i < 6; i++)
faces [i] = cube.face [i].square [1, 1];
return faces;
}
public static void Main(string[] args)
{
Motor motor = new Motor (MotorPort.OutA);
motor.ResetTacho();
motor.MoveTo(25,1000,true);
System.Threading.Thread.Sleep(3000);
Console.WriteLine(motor.GetTachoCount().ToString());
/*motor.MoveTo(75,0,false,true);
LcdConsole.WriteLine(motor.GetTachoCount().ToString());
System.Threading.Thread.Sleep(3000);
LcdConsole.WriteLine ("Done executing motor test");*/
}
/// <summary>
/// Repeatedly checks the state of the switch and resets it necessary
/// </summary>
private static void MainLoop(Motor motor)
{
var touchSensor = new TouchSensor(SensorPort.In1);
while (true) {
// there are a few states that the switch and the arm can be in
// our default is the arm down, and the switch off
// if someone flips the switch, we'll move our arm up until it completes its motion
// at that point, the switch will be off again and we'll move the arm back
// Our "switch" is off when the touch sensor is pressed (inverse)
var switchStatus = !touchSensor.IsPressed ();
// When our switch is on, we need to turn it back off
if (switchStatus) {
// if the motor hasn't moved backwards to push the switch off, we'll do that
if (motor.GetTachoCount () > -120) {
// LcdConsole.WriteLine ("(On) Extending arm");
motor.On (-20);
System.Threading.Thread.Sleep (50);
} else { // if it's moved all the way back, stop pushing it
// LcdConsole.WriteLine ("(On) Arm fully extended");
motor.Off ();
}
} else { // When it's off, we're happy
// if the arm isn't fully retracted, it's time to put it back to zero
if (motor.GetTachoCount () < -20) {
// LcdConsole.WriteLine ("(Off) Reset the arm");
motor.On (20);
System.Threading.Thread.Sleep (200);
} else { // if it is, we're done
// LcdConsole.WriteLine ("(Off) Arm fully reset");
motor.Off ();
}
}
if (switchStatus != PreviouslySwitched && !PreviouslySwitched) {
Action<string> foo = delegate(string x) {
HttpWebRequest request = (HttpWebRequest)WebRequest.Create ("http://crappychatws.azurewebsites.net/api/lego");
request.Proxy = null;
request.GetResponse ();
LcdConsole.WriteLine(x);
};
foo.BeginInvoke ("Someone flipped my switch >_<", null, null);
}
PreviouslySwitched = switchStatus;
}
}
public static void switchMode()
{
Motor motorSwitch = new Motor(MotorPort.OutA);
if (extensionMode) {
LcdConsole.WriteLine ("Switching to height adjustment");
motorSwitch.On (30, 1550, true);
extensionMode = false;
} else {
LcdConsole.WriteLine ("Switching to extension mode");
motorSwitch.On (-30, 1500, true);
extensionMode = true;
}
motorSwitch.Off ();
}
public static void Main (string[] args)
{
Motor motor = new Motor(MotorPort.OutA);
motor.ResetTacho();
PositionPID PID = new PositionPID(motor, 4000, true, 50, P, I, D, 5000);
var waitHandle = PID.Run();
Console.WriteLine("Moving motor A to position 4000");
Console.WriteLine("Waiting for controller to finish");
//Wait for controller to finish - you can do other stuff here
waitHandle.WaitOne();
Console.WriteLine("Done");
Console.WriteLine("Motor position: " + motor.GetTachoCount());
}
// センサ/モータ
// タッチセンサ 1
// 超音波センサ 2
// カラーセンサ 3
// ジャイロセンサ 4
// 尻尾用モータ A
// 右車輪用モータ B
// 左車輪用モータ C
public EquipmentParameters ()
{
// 車両情報
// ポート番号
TailMoter = new Motor(MotorPort.OutA);
MpRight = new Motor(MotorPort.OutB);
MpLeft = new Motor(MotorPort.OutC);
TouchSensors = new EV3TouchSensor(SensorPort.In1);
// TODO : 超音波センサーはLのみ(Primary)
DistSensors = new EV3UltrasonicSensor(SensorPort.In2, UltraSonicMode.Centimeter);
// 反射
LightSensors = new EV3ColorSensor(SensorPort.In3, ColorMode.Reflection);
GyroSensors = new EV3GyroSensor(SensorPort.In4, GyroMode.AngularVelocity);
}
/*
* Constructor for the real cube through Lego robot
*/
public Cube(Motor motorA, Motor motorB, Motor motorD)
{
this.motorA = motorA;
this.motorB = motorB;
this.motorD = motorD;
this.motorA.ResetTacho();
this.motorB.ResetTacho();
this.motorD.ResetTacho();
this._isArmRest = true;
this.face = new Face [] {
new Face ('O'),
new Face ('B'),
new Face ('W'),
new Face ('G'),
new Face ('Y'),
new Face ('R')
};
}
public static void Main(string[] args)
{
ManualResetEvent terminateProgram = new ManualResetEvent(false);
Motor motor = new Motor (MotorPort.OutA);
// pull the arm back slightly to the starting position
ResetArmLocation (motor);
// a loop needs to run to watch the arm and reset the switch if someone is rude enough to push it
Task.Factory.StartNew(() => MainLoop(motor));
// Setup the down button to terminate the app
ButtonEvents buts = new ButtonEvents ();
buts.DownPressed += () => {
motor.Off();
terminateProgram.Set();
};
terminateProgram.WaitOne();
}
public static void Main(string[] args)
{
Console.WriteLine ("Resetting the motors");
TouchSensor ts = new TouchSensor(SensorPort.In2);
Motor motorFwd = new Motor(MotorPort.OutB);
Motor motorTurn = new Motor(MotorPort.OutC);
Motor motorSwitch = new Motor(MotorPort.OutA);
Motor motorArm = new Motor(MotorPort.OutD);
motorArm.On (-60);
ButtonEvents buts = new ButtonEvents();
bool keepGoing = true;
while (keepGoing) {
buts.EscapePressed += () => {
keepGoing = false;
};
if (ts.IsPressed ()) {
keepGoing = false;
}
}
motorArm.Off ();
switchMode ();
keepGoing = true;
TouchSensor ts2 = new TouchSensor(SensorPort.In4);
motorArm.On (-60);
while (keepGoing) {
buts.EscapePressed += () => {
keepGoing = false;
};
if (ts2.IsPressed ()) {
keepGoing = false;
}
}
motorArm.Off ();
switchMode ();
motorArm.ResetTacho ();
motorFwd.ResetTacho ();
motorSwitch.ResetTacho ();
motorTurn.ResetTacho ();
}
public MotorModel (MonoBrickFirmware.Movement.MotorPort port)
{
Port = port.ToString();
motor = new Motor(port);
Update();
}
public virtual void Stay()
{
Motor m = new Motor(MotorPort.OutC);
m.SpeedProfileTime(-30, 70, 4000, 70, true);
System.Threading.Thread.Sleep(4000 );
//TODO: Make the right moves
MotorSync ms = new MotorSync(MotorPort.OutA, MotorPort.OutD);
ms.TimeSync(-99, 0, 1000, true);
}
public virtual void HeadDown()
{
//TODO: Make the right moves
Motor m = new Motor(MotorPort.OutC);
m.SpeedProfileTime(30, 70, 700, 70, true);
}
/// <summary>
/// Resets the useless arm to its initial location (down)
/// </summary>
private static void ResetArmLocation(Motor motor)
{
LcdConsole.WriteLine ("Resetting the arm");
motor.On (20);
// there is a ramp up time for the motor. if the speed is checked
// to fast then it will still be zero
System.Threading.Thread.Sleep (200);
// we'll be using the tachometer to track rotations so lets reset it
motor.ResetTacho ();
System.Threading.Thread.Sleep (100);
while (motor.GetSpeed() > 0) {
// just keeps us from moving on until the arm is fully reset
// hacky I know
}
LcdConsole.WriteLine ("Arm reset");
motor.Off();
}
/*
* This method moves a motor relatively to its own position.
* Although it can be used with any motor, only the cube tray
* is in fact invoking this method to rotate in 45, 90 or 180
* degrees.
*/
public static void MoveRel(Motor a, int relpos, sbyte speed = 90)
{
WaitHandle handle;
try {
if (relpos < 0)
handle = a.SpeedProfile (speed, 0, (uint)-relpos-40, 40, true);
else
handle = a.SpeedProfile ((sbyte)-speed, 0, (uint) relpos-40, 40, true);
handle.WaitOne ();
}
catch (WaitHandleCannotBeOpenedException e) {
Console.WriteLine (e.ToString ());
}
}
/*
* This method moves a motor in absolute positions.
* Although it can be used with any motor, only the
* sensor arm and the cube arm are in fact using it.
*/
public static void Move(Motor a, int abspos, sbyte speed = 30)
{
int relpos;
WaitHandle handle;
relpos = a.GetTachoCount () - abspos;
try {
if (relpos < 0)
handle = a.SpeedProfile (speed, 4, (uint)-relpos-8, 4, true);
else
handle = a.SpeedProfile ((sbyte)-speed, 4, (uint) relpos-8, 4, true);
handle.WaitOne ();
}
catch (WaitHandleCannotBeOpenedException e) {
Console.WriteLine (e.ToString ());
}
}
public static RGBColor[] CountFacelets(Motor a, EV3ColorSensor sensor)
{
RGBColor[] colors = new RGBColor[8];
a.ResetTacho();
for (int count = 0; count < 8;)
if (count == a.GetTachoCount () / 45 % 8)
colors [count++] = sensor.ReadRGB ();
return colors;
}
/*
* This method controls the movements of the robot to scan the facelets
* of any single face of the cube.
*/
public static void BuildFace(Face face, Motor motorA, Motor motorB, EV3ColorSensor sensor)
{
RGBColor[] colors = null;
RGBColor color = null;
Thread count = new Thread (() => colors = CountFacelets (motorA, sensor));
MoveCube.Move (motorB, MoveCube.SensorMid, 15);
color = sensor.ReadRGB ();
MoveCube.Move (motorB, MoveCube.SensorSide, 15);
count.Start ();
while (!count.IsAlive);
WaitHandle handle = motorA.SpeedProfile ((sbyte)100, 0, (uint) 360, 0, true);
handle.WaitOne ();
count.Join ();
MoveCube.Move (motorB, MoveCube.SensorRest, 15);
char colorvalue = ' ';
for (int i = 0; i < 9; i++) {
switch (i) {
case 0:
colorvalue = getColorValue (color);
face.square[1,1] = colorvalue;
break;
case 1:
color = colors[0];
colorvalue = getColorValue (color);
face.square[2,1] = colorvalue;
break;
case 3:
color = colors[2];
colorvalue = getColorValue (color);
face.square[1,2] = colorvalue;
break;
case 5:
color = colors[4];
colorvalue = getColorValue (color);
face.square[0,1] = colorvalue;
break;
case 7:
color = colors[6];
colorvalue = getColorValue (color);
face.square[1,0] = colorvalue;
break;
case 2:
color = colors[1];
colorvalue = getColorValue (color);
face.square[2,2] = colorvalue;
break;
case 4:
color = colors[3];
colorvalue = getColorValue (color);
face.square[0,2] = colorvalue;
break;
case 6:
color = colors[5];
colorvalue = getColorValue (color);
face.square[0,0] = colorvalue;
break;
case 8:
color = colors[7];
colorvalue = getColorValue (color);
face.square[2,0] = colorvalue;
break;
}
Console.WriteLine ("Line: {0} Color: {1} RGB code: {2}", i, colorvalue, color);
}
}
public virtual void Pee()
{
//Lift leg
Motor motor = new Motor(MotorPort.OutA);
motor.ResetTacho();
motor.SetSpeed(20);
System.Threading.Thread.Sleep(2500);
motor.Brake();
//System.Threading.Thread.Sleep(2500);
//Make peeing sound
MakeSound(DogSound.Pee);
LcdConsole.WriteLine("Peeing");
//Reset bladder
BladderLevel = 0;
}
public static void Main (string[] args)
{
Motor motorA = new Motor (MotorPort.OutA);
Motor motorD = new Motor (MotorPort.OutD);
WaitHandle motorWaitHandle;
motorA.Off();
motorD.Off();
//Power control
LcdConsole.WriteLine("Set power to 50");
motorA.SetPower(50);
Thread.Sleep(3000);
LcdConsole.WriteLine("Break");
motorA.Brake();
//Speed control
LcdConsole.WriteLine("Set speed to 50");
motorA.SetSpeed(50);
Thread.Sleep(1000);
LcdConsole.WriteLine("Speed: " + motorA.GetSpeed());
Thread.Sleep(2000);
LcdConsole.WriteLine("Break");
motorA.Brake();
//Position control of single motor
Thread.Sleep(3000);
motorA.ResetTacho();
LcdConsole.WriteLine("Moving motor A to 2000 ");
motorWaitHandle = motorA.SpeedProfile(50, 200, 1600, 200,true);
//you could do something else here
LcdConsole.WriteLine("Waiting for motor A to stop");
motorWaitHandle.WaitOne();
LcdConsole.WriteLine("Done moving motor");
LcdConsole.WriteLine("Position A: " + motorA.GetTachoCount());
//Individual position control of both motors
Thread.Sleep(3000);
motorA.ResetTacho();
motorD.ResetTacho();
LcdConsole.WriteLine("Moving motors A to 2000");
LcdConsole.WriteLine("Moving motor B to 1000");
WaitHandle[] handles = new WaitHandle[2];
handles[0] = motorA.SpeedProfile(50, 200, 1600, 200,true);
handles[1] = motorD.SpeedProfile(50,200,600,200,true);
//you could do something else here
LcdConsole.WriteLine("Waiting for both motors to stop");
WaitHandle.WaitAll(handles);
LcdConsole.WriteLine("Done moving both motors");
LcdConsole.WriteLine("Position A: " + motorA.GetTachoCount());
LcdConsole.WriteLine("Position D: " + motorD.GetTachoCount());
motorA.Off();
motorD.Off();
//Motor synchronisation position control
Thread.Sleep(3000);
motorA.ResetTacho();
motorD.ResetTacho();
MotorSync sync = new MotorSync(MotorPort.OutA, MotorPort.OutD);
LcdConsole.WriteLine("Sync motors to move 3000 steps forward");
motorWaitHandle = sync.StepSync(40,0, 3000, true);
//you could do something else here
LcdConsole.WriteLine("Waiting for sync to stop");
motorWaitHandle.WaitOne();
LcdConsole.WriteLine("Done sync moving both motors");
LcdConsole.WriteLine("Position A: " + motorA.GetTachoCount());
LcdConsole.WriteLine("Position D: " + motorD.GetTachoCount());
sync.Off();
}
