/*
* This method turns the virtual cube and the real cube (if instantiated)
* 90 degrees counterclockwise.
*/
public void TurnCubeCCW()
{
// moves the virtual model of the cube
Face aux;
this.face [0].TurnCCWCore();
this.face [1].TurnCCWCore();
this.face [2].TurnCCWCore();
this.face [3].TurnCCWCore();
this.face [4].TurnCCWCore();
this.face [5].TurnCWCore();
aux = this.face [1];
this.face [1] = this.face [2];
this.face [2] = this.face [3];
this.face [3] = this.face [4];
this.face [4] = aux;
// actually moves the physical cube
if (motorA != null && motorB != null && motorD != null)
{
if (!this._isArmRest)
{
MoveCube.Move(motorD, MoveCube.RestArm);
this._isArmRest = true;
}
MoveCube.MoveRel(motorA, MoveCube.CCW90);
}
}
/*
* This method turns the bottom face of the virtual cube and the real cube
* (if instantiated) 180 degrees.
*/
public void TurnRow180()
{
// moves the virtual model of the cube
char aux;
for (int i = 0; i < 2; i++)
{
this.face [0].TurnCWCore();
aux = this.face [1].square [2, 2];
this.face [1].square [2, 2] = this.face [4].square [0, 2];
this.face [4].square [0, 2] = this.face [3].square [0, 0];
this.face [3].square [0, 0] = this.face [2].square [2, 0];
this.face [2].square [2, 0] = aux;
aux = this.face [1].square [1, 2];
this.face [1].square [1, 2] = this.face [4].square [0, 1];
this.face [4].square [0, 1] = this.face [3].square [1, 0];
this.face [3].square [1, 0] = this.face [2].square [2, 1];
this.face [2].square [2, 1] = aux;
aux = this.face [1].square [0, 2];
this.face [1].square [0, 2] = this.face [4].square [0, 0];
this.face [4].square [0, 0] = this.face [3].square [2, 0];
this.face [3].square [2, 0] = this.face [2].square [2, 2];
this.face [2].square [2, 2] = aux;
}
// actually moves the physical cube
if (motorA != null && motorB != null && motorD != null)
{
MoveCube.Move(motorD, MoveCube.GrabArm);
MoveCube.MoveRel(motorA, MoveCube.CW180);
this._isArmRest = false;
}
}
/*
* This method puts the cube arm in rest position.
*/
public void RestArm()
{
if (motorA != null && motorB != null && motorD != null)
{
MoveCube.Move(motorD, MoveCube.RestArm);
this._isArmRest = true;
}
}
/*
* This method rolls the virtual and real cube (if instantiated).
*/
public void Roll()
{
// moves the virtual model of the cube
Face aux;
this.face [1].TurnCCWCore();
this.face [3].TurnCWCore();
aux = this.face [2];
this.face [2] = this.face [0];
this.face [0] = this.face [4];
this.face [4] = this.face [5];
this.face [5] = aux;
// actually moves the physical cube
if (motorA != null && motorB != null && motorD != null)
{
MoveCube.Move(motorD, MoveCube.GrabArm);
MoveCube.Move(motorD, MoveCube.PullArm);
MoveCube.Move(motorD, MoveCube.GrabArm, 50);
this._isArmRest = false;
}
}
/*
* 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);
}
/*
* 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);
}
}
