/*
* 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 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);
}
}
/*
* 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 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();
}