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