Tilted Twister WORKING 100% for NXT 2.0 Color Sensor :-) Options

[dodgey99]

Well, here is is! After days of dicking about and getting my head round NXC and tring to figure out how to get the Tilted Twister working with the NXT 2.0 color sensor I cracked it! And boy was it a simple solution lol.

Changing the code to read the new sensor was easy, but it kept getting too inconsistent readings on the r g and b values - which is then uses to determine which colour is which. I put pauses in the code to see the rgb values, and also, importantly , the colorval (the NXT 2 color sensor outputs a value of 1 to 6 for the 6 colors it recognises). I noticed that the rgb values varied quite a lot, particularly when reading a center color compared to an edge color. BUT, I also noticed that the colorval (1-6) was ALWAYS correct.

The problem was that the entire program is written around looking at the r g and b values (red green and blue) and comparing differences to find a best match, so I was stumped as to how to replace the r g and b values with a simple one digit color number. Then it hit me! Why not just put the colorval into the r g and b variables and then let the program compare them as if they were rgb values. Who cares what the values are, as long as they are consistent!

So now, for example, when it scans a white face, instead of the r g and b values being something like 460,407,362 I have made it so it puts the colorval in there, so now r g and b are 6,6,6 (6 is the colorval for white). There are no inconsistencies to iron out at all.

You don't really even need to calibrate now, as the colorvals never change. The only thing it helps with is aligning the color sensor.

I ran this new code and it worked 1st time. Wonderful!

The physical mods I made were loosening the clamp as many people have suggested, and I added a new vertical arm to support the sensor better, but it's not needed. My sensor is 5mm above the cube face - just set it so the white lamp fills the square with no overlap. I still have to dive in and straighten the cube occasionally but I'll iron that out eventually. Oh, importantly, I stuck black stickers over the orange squares on the cube. NTX 2 sensor doesn't detect orange natively.

1st, here is the calibrate program - once you know the position of the sensor you never need to run this again really. There are pauses that require the orange button to be pushed. These are so you can check out your sensor readings - if you want rid of the pauses just remove "while(ButtonPressed(BTNCENTER,false)==0);" from line 146

CODE

/*
Calibrate2
Hans Andersson 2008
www.tiltedtwister.com

Modified 2009 by Mike O'Conner, then Roger Donoghue Jan 2010 to insert NXT 2 color sensor support

Inputs:
1 Touch sensor
2 Color sensor

Outputs:
A Center motor
B Tilter arm motor
C Color sensor motor
*/

#include "NXCDefs.h"

#define LEFTFACE      0
#define FRONTFACE     1
#define RIGHTFACE     2
#define BACKFACE      3
#define UPPERFACE     4
#define DOWNFACE      5

#define OFFSETC       35

#define COLOR_SENSOR_MOTOR_SLOW    20

// Power of tilt
#define TILTPOWER1  50
#define TILTPOWER2  100

// Angles of power1 and power2  TILTANGLE1 + TILTANGLE2 = 63
#define TILTANGLE1  35
#define TILTANGLE2  28

// Power of returning tilterarm
#define TILTRETURNPOWER1  100
#define TILTRETURNPOWER2  20

// Angles of power1 and power2  TILTRETURNANGLE1 + TILTRETURNANGLE2 = -63
#define TILTRETURNANGLE1  -33
#define TILTRETURNANGLE2  -30

// Time (ms) to rest before return tilterarm
#define TILTREST  200

bool colorSensor;
int tachoCenter;
int red[6];
int green[6];
int blue[6];


//void Tilt_Old()
//{
//  RotateMotorEx(OUT_B,50,40,0,false,false);
//  RotateMotorEx(OUT_B,100,15,0,false,false);
//  RotateMotor(OUT_B,40,10);
//  RotateMotor(OUT_B,30,-65);
//}


// Taken from tiltedtwister source
void Tilt()
{
  RotateMotorEx(OUT_B,TILTPOWER1,TILTANGLE1,0,false,false);
  RotateMotorEx(OUT_B,TILTPOWER2,TILTANGLE2,0,false,false);
  RotateMotorEx(OUT_B,50,-1,0,false,false);
  RotateMotorEx(OUT_B,100,1,0,false,true);
  Wait(TILTREST);
  RotateMotorEx(OUT_B,TILTRETURNPOWER1,TILTRETURNANGLE1,0,false,false);
  RotateMotor(OUT_B,TILTRETURNPOWER2,TILTRETURNANGLE2);
}


void ReadColor(int face)
{
//    long r,g,b,color_num;
    unsigned long raw[];
    unsigned long norm[];
    int scaled[], colorval;
    while(!ReadSensorColorEx(IN_2, colorval, raw, norm, scaled));
    red[face]+=colorval;
    green[face]+=colorval;
    blue[face]+=colorval;
    TextOut(10,LCD_LINE3,"RED   =",true);
    TextOut(10,LCD_LINE4,"GREEN =");
    TextOut(10,LCD_LINE5,"BLUE  =");
    NumOut(60,LCD_LINE3,colorval);
    NumOut(60,LCD_LINE4,colorval);
    NumOut(60,LCD_LINE5,colorval);
}


void TurnQuarter(bool coast)
{
  RotateMotorExPID(OUT_A,100,315,false,false,!coast,40,40,90);
}


void ScanFace(int face, string title, int preTurn, int postTurn)
{
  TextOut(20,LCD_LINE2,title,true);
//Center
  RotateMotor(OUT_C,20,tachoCenter - OFFSETC);
  ReadColor(face);
//Edges
  RotateMotor(OUT_C,20,-20);
  RotateMotorExPID(OUT_A,100,60+preTurn*315,false,false,colorSensor,40,40,90);
  ReadColor(face);
  TurnQuarter(!colorSensor);
  ReadColor(face);
  TurnQuarter(!colorSensor);
  ReadColor(face);
  TurnQuarter(false);
  ReadColor(face);
//Corners
  RotateMotor(OUT_C,20,-8);
  RotateMotorExPID(OUT_A,100,160,false,false,colorSensor,40,40,90);
  ReadColor(face);
  TurnQuarter(!colorSensor);
  ReadColor(face);
  TurnQuarter(!colorSensor);
  ReadColor(face);
  TurnQuarter(!colorSensor);
  ReadColor(face);
  RotateMotorPID(OUT_A,100,95+postTurn*315,40,40,90);
  RotateMotor(OUT_C,20,-1 * tachoCenter + 30 + OFFSETC);
}


void PrintValuesFace(int face, string title)
{
  TextOut(10,LCD_LINE1,title,true);
  TextOut(10,LCD_LINE2,"RED   =",true);
  TextOut(10,LCD_LINE3,"GREEN =");
  TextOut(10,LCD_LINE4,"BLUE  =");
  TextOut(0,LCD_LINE5,"Press orange btn");
  TextOut(0,LCD_LINE6,"to continue");
  NumOut(60,LCD_LINE2,red[face]);
  NumOut(60,LCD_LINE3,green[face]);
  NumOut(60,LCD_LINE4,blue[face]);
  while(ButtonPressed(BTNCENTER,false)==0);
  Wait(2000);
  PlayTone(500,1);
  ClearScreen();
}

void CalibrateColors()
{
  ScanFace(LEFTFACE,"LEFT FACE",0,0);
  Tilt();
  PrintValuesFace(LEFTFACE,"LEFT FACE");
  ScanFace(BACKFACE,"BACK FACE",1,0);
  Tilt();
  PrintValuesFace(BACKFACE,"BACK FACE");
  ScanFace(RIGHTFACE,"RIGHT FACE",1,3);
  Tilt();
  PrintValuesFace(RIGHTFACE,"RIGHT FACE");
  ScanFace(DOWNFACE,"DOWN FACE",1,1);
  Tilt();
  PrintValuesFace(DOWNFACE,"DOWN FACE");
  ScanFace(FRONTFACE,"FRONT FACE",0,1);
  Tilt();
  PrintValuesFace(FRONTFACE,"FRONT FACE");
  ScanFace(UPPERFACE,"UPPER FACE",0,0);
  PrintValuesFace(UPPERFACE,"UPPER FACE");
  Wait(2000);
}


void SaveCalibration()
{
  int handle;
  if(colorSensor)
  {
    DeleteFile("tiltedtwister.clr")
    CreateFile("tiltedtwister.clr",2+6*3*2,handle);
    Write(handle,tachoCenter);
    for(int face=0;face<6;face++)
    {
      int value;
      value=red[face]/8;
      Write(handle,value);
      value=green[face]/8;
      Write(handle,value);
      value=blue[face]/8;
      Write(handle,value);
    }
    CloseFile(handle);
  }
  else
  {
    DeleteFile("tiltedtwister.lgt")
    CreateFile("tiltedtwister.lgt",2,handle);
    Write(handle,tachoCenter);
    CloseFile(handle);
  }
}


void CalibrateSensorPosition()
{
  TextOut(0,LCD_LINE1,"Position sensor",true);
  TextOut(0,LCD_LINE2,"above center");
  TextOut(0,LCD_LINE3,"of cube");
  TextOut(0,LCD_LINE5,"Press orange btn");
  TextOut(0,LCD_LINE6,"to continue");
  while(ButtonPressed(BTNCENTER,false)==0);
  PlayTone(500,1);
  ClearScreen();
  Wait(2000);
  ResetTachoCount(OUT_C);
  OnRev(OUT_C,COLOR_SENSOR_MOTOR_SLOW);
  until(SENSOR_1 == 1);
  Off(OUT_C);
  tachoCenter = abs(MotorTachoCount(OUT_C));
  NumOut(30,LCD_LINE3,tachoCenter);
  RotateMotor(OUT_C,COLOR_SENSOR_MOTOR_SLOW,OFFSETC);
  Wait(2000);
}


void Initialize()
{
  SetSensorTouch(IN_1);
  SetSensorColorFull(IN_2);
  SetSensorMode(IN_2,SENSOR_MODE_RAW);
  colorSensor = true;
  SetSensorType(IN_2, IN_TYPE_LOWSPEED);
  SetSensorMode(IN_2, IN_MODE_RAW);
  SetSensorColorFull(IN_2)
}


task main()
{
  Initialize();
  CalibrateSensorPosition();
  if(colorSensor)
    CalibrateColors();
  SaveCalibration();
  TextOut(20,LCD_LINE3,"CALIBRATION",true);
  TextOut(20,LCD_LINE5," FINISHED");
  PlayTone(500,100);
  while(ButtonPressed(BTNCENTER,false)==0);
}

[dodgey99]

And here is the main tiltedtwister app:

CODE

/*
Tilted Twister, a Lego Mindstorms robot that solves Rubik’s cube
Hans Andersson 2008
http://tiltedtwister.com

Version 1.4 (2008-09-21)

Modified by Mike O'Conner, 2009, then Roger Donoghue Jan 2010 to add NXT 2.0 color sensor support

Inputs:
1 Touch sensor
2 Light sensor or  Color sensor
3 Ultrasonic sensor

Outputs:
A Center motor
B Tilter arm motor
C Light/Color sensor motor
*/


///////////////////////////////////////////////////////////////////////////////

//Ultrasonic Sensor Limits
#define US_MIN        7
//#define US_MAX        9
#define US_MAX        10

// Power of tilt
#define TILTPOWER1  50
//#define TILTPOWER1   70
#define TILTPOWER2  100

// Angles of power1 and power2  TILTANGLE1 + TILTANGLE2 = 63
#define TILTANGLE1  35
#define TILTANGLE2  28

// Power of returning tilterarm
#define TILTRETURNPOWER1  100
#define TILTRETURNPOWER2  20

// Angles of power1 and power2  TILTRETURNANGLE1 + TILTRETURNANGLE2 = -63
#define TILTRETURNANGLE1  -33
#define TILTRETURNANGLE2  -30

// Time (ms) to rest before return tilterarm
#define TILTREST  200
//#define TILTREST    1000

// Time (ms) to rest after a turn
#define TURNREST    1000
#define GRABREST    1000
#define RELEASEREST 1000

// Angle to overtwist
//#define OVERTWIST     15
#define OVERTWIST     17

// Minimum power to color sensor motor
#define COLOR_SENSOR_MOTOR_SLOW    20

#define P 50
#define I 40
#define D 80

///////////////////////////////////////////////////////////////////////////////

#define LEFTFACE      0
#define FRONTFACE     1
#define RIGHTFACE     2
#define BACKFACE      3
#define UPPERFACE     4
#define DOWNFACE      5

#define  UPPERLEFT    0
#define  UPPERMID     1
#define  UPPERRIGHT   2
#define  MIDLEFT      3
#define  CENTER       4
#define  MIDRIGHT     5
#define  DOWNLEFT     6
#define  DOWNMID      7
#define  DOWNRIGHT    8

#define  LEFTFACE_UPPERLEFT   0
#define  LEFTFACE_UPPERMID    1
#define  LEFTFACE_UPPERRIGHT  2
#define  LEFTFACE_MIDLEFT     3
#define  LEFTFACE_CENTER      4
#define  LEFTFACE_MIDRIGHT    5
#define  LEFTFACE_DOWNLEFT    6
#define  LEFTFACE_DOWNMID     7
#define  LEFTFACE_DOWNRIGHT   8

#define  FRONTFACE_UPPERLEFT  9
#define  FRONTFACE_UPPERMID   10
#define  FRONTFACE_UPPERRIGHT 11
#define  FRONTFACE_MIDLEFT    12
#define  FRONTFACE_CENTER     13
#define  FRONTFACE_MIDRIGHT   14
#define  FRONTFACE_DOWNLEFT   15
#define  FRONTFACE_DOWNMID    16
#define  FRONTFACE_DOWNRIGHT  17

#define  RIGHTFACE_UPPERLEFT  18
#define  RIGHTFACE_UPPERMID   19
#define  RIGHTFACE_UPPERRIGHT 20
#define  RIGHTFACE_MIDLEFT    21
#define  RIGHTFACE_CENTER     22
#define  RIGHTFACE_MIDRIGHT   23
#define  RIGHTFACE_DOWNLEFT   24
#define  RIGHTFACE_DOWNMID    25
#define  RIGHTFACE_DOWNRIGHT  26

#define  BACKFACE_UPPERLEFT   27
#define  BACKFACE_UPPERMID    28
#define  BACKFACE_UPPERRIGHT  29
#define  BACKFACE_MIDLEFT     30
#define  BACKFACE_CENTER      31
#define  BACKFACE_MIDRIGHT    32
#define  BACKFACE_DOWNLEFT    33
#define  BACKFACE_DOWNMID     34
#define  BACKFACE_DOWNRIGHT   35

#define  UPPERFACE_UPPERLEFT  36
#define  UPPERFACE_UPPERMID   37
#define  UPPERFACE_UPPERRIGHT 38
#define  UPPERFACE_MIDLEFT    39
#define  UPPERFACE_CENTER     40
#define  UPPERFACE_MIDRIGHT   41
#define  UPPERFACE_DOWNLEFT   42
#define  UPPERFACE_DOWNMID    43
#define  UPPERFACE_DOWNRIGHT  44

#define  DOWNFACE_UPPERLEFT   45
#define  DOWNFACE_UPPERMID    46
#define  DOWNFACE_UPPERRIGHT  47
#define  DOWNFACE_MIDLEFT     48
#define  DOWNFACE_CENTER      49
#define  DOWNFACE_MIDRIGHT    50
#define  DOWNFACE_DOWNLEFT    51
#define  DOWNFACE_DOWNMID     52
#define  DOWNFACE_DOWNRIGHT   53

#define OFFSETC       35

#define MOVES_SIZE  500

int staticOrientations[]={
LEFTFACE,FRONTFACE,RIGHTFACE,BACKFACE,UPPERFACE,DOWNFACE,
FRONTFACE,RIGHTFACE,BACKFACE,LEFTFACE,UPPERFACE,DOWNFACE,
RIGHTFACE,BACKFACE,LEFTFACE,FRONTFACE,UPPERFACE,DOWNFACE,
BACKFACE,LEFTFACE,FRONTFACE,RIGHTFACE,UPPERFACE,DOWNFACE,

DOWNFACE,FRONTFACE,UPPERFACE,BACKFACE,LEFTFACE,RIGHTFACE,
FRONTFACE,UPPERFACE,BACKFACE,DOWNFACE,LEFTFACE,RIGHTFACE,
UPPERFACE,BACKFACE,DOWNFACE,FRONTFACE,LEFTFACE,RIGHTFACE,
BACKFACE,DOWNFACE,FRONTFACE,UPPERFACE,LEFTFACE,RIGHTFACE,

LEFTFACE,DOWNFACE,RIGHTFACE,UPPERFACE,FRONTFACE,BACKFACE,
DOWNFACE,RIGHTFACE,UPPERFACE,LEFTFACE,FRONTFACE,BACKFACE,
RIGHTFACE,UPPERFACE,LEFTFACE,DOWNFACE,FRONTFACE,BACKFACE,
UPPERFACE,LEFTFACE,DOWNFACE,RIGHTFACE,FRONTFACE,BACKFACE,

FRONTFACE,DOWNFACE,BACKFACE,UPPERFACE,RIGHTFACE,LEFTFACE,
DOWNFACE,BACKFACE,UPPERFACE,FRONTFACE,RIGHTFACE,LEFTFACE,
BACKFACE,UPPERFACE,FRONTFACE,DOWNFACE,RIGHTFACE,LEFTFACE,
UPPERFACE,FRONTFACE,DOWNFACE,BACKFACE,RIGHTFACE,LEFTFACE,

LEFTFACE,UPPERFACE,RIGHTFACE,DOWNFACE,BACKFACE,FRONTFACE,
UPPERFACE,RIGHTFACE,DOWNFACE,LEFTFACE,BACKFACE,FRONTFACE,
RIGHTFACE,DOWNFACE,LEFTFACE,UPPERFACE,BACKFACE,FRONTFACE,
DOWNFACE,LEFTFACE,UPPERFACE,RIGHTFACE,BACKFACE,FRONTFACE,

LEFTFACE,BACKFACE,RIGHTFACE,FRONTFACE,DOWNFACE,UPPERFACE,
BACKFACE,RIGHTFACE,FRONTFACE,LEFTFACE,DOWNFACE,UPPERFACE,
RIGHTFACE,FRONTFACE,LEFTFACE,BACKFACE,DOWNFACE,UPPERFACE,
FRONTFACE,LEFTFACE,BACKFACE,RIGHTFACE,DOWNFACE,UPPERFACE};

int staticCorners[]={
UPPERFACE_UPPERLEFT,LEFTFACE_UPPERLEFT,BACKFACE_UPPERRIGHT,
UPPERFACE_UPPERRIGHT,BACKFACE_UPPERLEFT,RIGHTFACE_UPPERRIGHT,
UPPERFACE_DOWNLEFT,FRONTFACE_UPPERLEFT,LEFTFACE_UPPERRIGHT,
UPPERFACE_DOWNRIGHT,RIGHTFACE_UPPERLEFT,FRONTFACE_UPPERRIGHT,
DOWNFACE_UPPERLEFT,LEFTFACE_DOWNRIGHT,FRONTFACE_DOWNLEFT,
DOWNFACE_UPPERRIGHT,FRONTFACE_DOWNRIGHT,RIGHTFACE_DOWNLEFT,
DOWNFACE_DOWNLEFT,BACKFACE_DOWNRIGHT,LEFTFACE_DOWNLEFT,
DOWNFACE_DOWNRIGHT,RIGHTFACE_DOWNRIGHT,BACKFACE_DOWNLEFT};

int staticEdges[]={
UPPERFACE_UPPERMID,BACKFACE_UPPERMID,
UPPERFACE_MIDLEFT,LEFTFACE_UPPERMID,
UPPERFACE_MIDRIGHT,RIGHTFACE_UPPERMID,
UPPERFACE_DOWNMID,FRONTFACE_UPPERMID,
LEFTFACE_MIDRIGHT,FRONTFACE_MIDLEFT,
FRONTFACE_MIDRIGHT,RIGHTFACE_MIDLEFT,
RIGHTFACE_MIDRIGHT,BACKFACE_MIDLEFT,
BACKFACE_MIDRIGHT,LEFTFACE_MIDLEFT,
DOWNFACE_UPPERMID,FRONTFACE_DOWNMID,
DOWNFACE_MIDLEFT,LEFTFACE_DOWNMID,
DOWNFACE_MIDRIGHT,RIGHTFACE_DOWNMID,
DOWNFACE_DOWNMID,BACKFACE_DOWNMID};

byte color[6*9];
char cube[6*9];
char tmpCube[6*9];
char moves[MOVES_SIZE];
int  movesCount=0;
char solution[MOVES_SIZE];
int  solutionCount;
int  solutionTwists;
int  twists;
char staticfaces[]={'L','F','R','B','U','D'};
char faces[]={'L','F','R','B','U','D'};
bool colorSensor;
int tachoCenter;

int refLight[6];
int sensorLight[6*9];

struct rgb
{
  int red;
  int green;
  int blue;
};

rgb refColor[6];
rgb sensorColor[6*9];

unsigned int costMatrix[12*12];
int twistMatrix[12*12];

inline void CubeSet(int face, int pos, char value)
{
  cube[face * 9 + pos] = value;
}

inline char CubeGet(int face, int pos)
{
  return cube[face * 9 + pos];
}

inline char TmpCubeGet(int face, int pos)
{
  return tmpCube[face * 9 + pos];
}

inline void ColorSet(int face, int pos,int col)
{
  color[face*9+pos]=col;
}

inline int ColorGet(int face, int pos)
{
  return color[face*9+pos];
}

inline void SensorLightSet(int face, int pos,int light)
{
  sensorLight[face*9+pos]=light;
}

inline int SensorLightGet(int face, int pos)
{
  return sensorLight[face*9+pos];
}

inline void SensorColorSet(int face, int pos,int r, int g, int b)
{
  rgb col;
  col.red=r;
  col.green=g;
  col.blue=b;
  sensorColor[face*9+pos]=col;
}

inline void SensorColorGet(int face, int pos, rgb &col)
{
  col=sensorColor[face*9+pos];
}

inline int OrientationGet(int orientation, int face)
{
  return staticOrientations[orientation * 6 + face];
}

inline int CornerGet(int corner, int side)
{
  return staticCorners[corner * 3 + side];
}

inline int EdgeGet(int edge, int side)
{
  return staticEdges[edge * 2 + side];
}

inline void CostMatrixSet(int x, int y, unsigned int cost)
{
  costMatrix[x*12+y]=cost;
}

inline unsigned int CostMatrixGet(int x, int y)
{
  return costMatrix[x*12+y];
}

inline void TwistMatrixSet(int x, int y, int twist)
{
  twistMatrix[x*12+y]=twist;
}

inline int TwistMatrixGet(int x, int y)
{
  return twistMatrix[x*12+y];
}

int AlignmentError()
{
  long tacho=MotorTachoCount(OUT_A);
  int error=tacho%315;
  if(error > 157)
    return 315 - error;
  else
    return -1 * error;
}

void Align()
{
  int error=AlignmentError();
  if(abs(error) > 3)
      RotateMotorPID(OUT_A,100, error,P,I,D);
}

void CheckCube()
{
  int US=SensorUS(IN_3);
  if(US < US_MIN || US > US_MAX)
  {
    PlayFile("Woops.rso");
    while(true)
    {
      US=SensorUS(IN_3);
      if(US >= US_MIN && US <= US_MAX)
      {
        Wait(3000);
        US=SensorUS(IN_3);
        if(US >= US_MIN && US <= US_MAX)
          break;
      }
    }
    PlayFile("Thank You.rso");
    Wait(1000);
  }
}


void Turn(int num)
{
  CheckCube();
  int error=AlignmentError();
  if (num==3)
    RotateMotorPID(OUT_A,100,-315+error,P,I,D);
  else
    RotateMotorPID(OUT_A,100,315*num+error,P,I,D);

  Wait(TURNREST);

  for(int i=0;i<num;i++)
  {
    char lf=faces[LEFTFACE];
    faces[LEFTFACE]=faces[BACKFACE];
    faces[BACKFACE]=faces[RIGHTFACE];
    faces[RIGHTFACE]=faces[FRONTFACE];
    faces[FRONTFACE]=lf;
  }
}


void Tilt()
{
  CheckCube();
  RotateMotorEx(OUT_B,TILTPOWER1,TILTANGLE1,0,false,false);
  RotateMotorEx(OUT_B,TILTPOWER2,TILTANGLE2,0,false,false);
  RotateMotorEx(OUT_B,50,-1,0,false,false);
  RotateMotorEx(OUT_B,100,1,0,false,true);
  Wait(TILTREST);
  RotateMotorEx(OUT_B,TILTRETURNPOWER1,TILTRETURNANGLE1,0,false,false);
  RotateMotor(OUT_B,TILTRETURNPOWER2,TILTRETURNANGLE2);
  char uf=faces[UPPERFACE];
  faces[UPPERFACE]=faces[LEFTFACE];
  faces[LEFTFACE]=faces[DOWNFACE];
  faces[DOWNFACE]=faces[RIGHTFACE];
  faces[RIGHTFACE]=uf;
}


void GrabCube()
{
  RotateMotorEx(OUT_B,100,10,0,false,false);
  RotateMotor(OUT_B,40,30);
  Wait(GRABREST);
}


void ReleaseCube()
{
  RotateMotorEx(OUT_B,100,-10,0,false,false);
  RotateMotorEx(OUT_B,80,-10,0,false,false);
  RotateMotor(OUT_B,20,-20);
  Wait(RELEASEREST);
}


void Twist(int num)
{
  CheckCube();
  int error=AlignmentError();
  GrabCube();
  switch(num)
  {
    case 1:
      RotateMotorExPID(OUT_A,100,315+ OVERTWIST + error,0,false,false,P,I,D);
      RotateMotorExPID(OUT_A,100,-1*OVERTWIST,0,false,true,P,I,D);
      break;
    case 2:
      RotateMotorExPID(OUT_A,100,315 * 2 + OVERTWIST + error,0,false,false,P,I,D);
      RotateMotorExPID(OUT_A,100,-1*OVERTWIST,0,false,true,P,I,D);
      break;
    case    3:
      RotateMotorExPID(OUT_A,100,-315 - OVERTWIST + error,0,false,false,P,I,D);
      RotateMotorExPID(OUT_A,100,OVERTWIST,0,false,true,P,I,D);
      break;
  }
  Align();
  ReleaseCube();
}


void ReadColor(int face, int pos)
{
  //if(colorSensor)
  //{
  //  long r,g,b,color_num;
     unsigned long raw[];
     unsigned long norm[];
     int scaled[], colorval;
     while(!ReadSensorColorEx(IN_2, colorval, raw, norm, scaled));
     SensorColorSet(face,pos,colorval,colorval,colorval);
     NumOut(30,LCD_LINE4,colorval,true);
     NumOut(30,LCD_LINE5,colorval);
     NumOut(30,LCD_LINE6,colorval);
  /*}
  else
  {
    int light=Sensor(IN_2);
    SensorLightSet(face,pos,light);
    //NumOut(10+(pos%3)*30, 32 - (pos/3)*8,light);
  } */
}


void TurnQuarter(bool coast)
{
  RotateMotorExPID(OUT_A,100,315,false,false,!coast,40,40,90);
}


void ScanFace(int face, string title, int preTurn, int postTurn)
{
  TextOut(20,LCD_LINE2,title,true);
//Center
  RotateMotor(OUT_C,20,tachoCenter - OFFSETC);
  ReadColor(face,CENTER);
//Edges
  RotateMotor(OUT_C,20,-20);
  RotateMotorExPID(OUT_A,100,60+preTurn*315,false,false,colorSensor,40,40,90);
  ReadColor(face,DOWNMID);
  TurnQuarter(!colorSensor);
  ReadColor(face,MIDLEFT);
  TurnQuarter(!colorSensor);
  ReadColor(face,UPPERMID);
  TurnQuarter(false);
  ReadColor(face,MIDRIGHT);
//Corners
  RotateMotor(OUT_C,20,-9);
  RotateMotorExPID(OUT_A,100,160,false,false,colorSensor,40,40,90);
  ReadColor(face,DOWNRIGHT);
  TurnQuarter(!colorSensor);
  ReadColor(face,DOWNLEFT);
  TurnQuarter(!colorSensor);
  ReadColor(face,UPPERLEFT);
  TurnQuarter(!colorSensor);
  ReadColor(face,UPPERRIGHT);
  RotateMotorPID(OUT_A,100,95+postTurn*315,40,40,90);
  RotateMotor(OUT_C,20,-1 * tachoCenter + 30 + OFFSETC);
}


int CenterFit(int orientation)
{
  int fit=0;
  for(int face=0;face<6;face++)
    if(ColorGet(face,CENTER) == OrientationGet(orientation, face))
      fit++;
  return fit;
}


void ResolveCenterColors()
{
  TextOut(2,LCD_LINE4,"Center colors...");
  if(colorSensor)
  {
    for(int center=0;center<6;center++)
    {
      unsigned int minCost=$FFFF;
      int bestCubie;
      for(int cubie=0;cubie<6;cubie++)
      {
        rgb centerColor,cubieColor;
        centerColor = refColor[center];
        SensorColorGet(cubie,CENTER,cubieColor);
        unsigned int cost = (centerColor.red - cubieColor.red) * (centerColor.red - cubieColor.red) + (centerColor.green - cubieColor.green) * (centerColor.green - cubieColor.green) + (centerColor.blue - cubieColor.blue) * (centerColor.blue - cubieColor.blue);
        if(cost<minCost)
        {
          minCost=cost;
          bestCubie=cubie;
        }
      }
      ColorSet(center,CENTER,bestCubie);
    }

    int bestFit=0;
    int bestOrientation;
    for(int orientation=0;orientation<24;orientation++)
    {
      int fit=CenterFit(orientation);
      if(fit>bestFit)
      {
        bestFit=fit;
        bestOrientation=orientation;
      }
    }
    rgb tmpRefColor[6];
    ArraySubset(tmpRefColor,refColor,0,6);
    for(int face=0;face<6;face++)
    {
      rgb col=tmpRefColor[face];
      refColor[OrientationGet(bestOrientation, face)]=col;
    }

    for(int face=0;face<6;face++)
      ColorSet(face,CENTER,face);
  }
  else
  {
    for(int face=0;face<6;face++)
    {
      refLight[face] = SensorLightGet(face,CENTER);
      ColorSet(face,CENTER,face);
    }
  }
}


int BestFitCubie(int &cubie, int &twist, int dim)
{
  int bestX,bestY;
  unsigned int bestDiff=0;
  for(int x = 0; x < dim;x++)
  {
    unsigned int minCost=$FFFF;
    unsigned int min2Cost=$FFFF;
    int minY;
    for(int y=0; y < dim; y++)
      if(CostMatrixGet(x,y) < minCost)
      {
        minCost=CostMatrixGet(x,y);
        minY=y;
      }
    for(int y=0; y < dim; y++)
      if(y != minY && CostMatrixGet(x,y) < min2Cost)
        min2Cost=CostMatrixGet(x,y);
    if(min2Cost-minCost > bestDiff)
    {
      bestX = x;
      bestY = minY;
      bestDiff = min2Cost-minCost;
    }
  }
  for(int x=0;x<dim;x++)
    CostMatrixSet(x,bestY,$FFFF);
  for(int y=0;y<dim;y++)
    CostMatrixSet(bestX,y,$FFFF);

  cubie = bestY;
  twist = TwistMatrixGet(bestX,bestY);
  return bestX;
}


void ResolveCornerColors()
{
  TextOut(2,LCD_LINE5,"Corner colors...");
  for(int corner=0;corner<8;corner++)
    for(int cubie=0;cubie<8;cubie++)
    {
      unsigned int minCost=$FFFF;
      int minCostTwist;
      for(int twist=0;twist<3;twist++)
      {
        unsigned int cost=0;
        for(int s=0;s<3;s++)
        {
          int face = CornerGet(corner,s) / 9;
          int pos = CornerGet(cubie,(s+twist)%3);
          if(colorSensor)
          {
            rgb color1,color2;
            color1 = refColor[face];
            color2 = sensorColor[pos];
            cost += ((color1.red - color2.red) * (color1.red - color2.red) + (color1.green - color2.green) * (color1.green - color2.green) + (color1.blue - color2.blue) * (color1.blue - color2.blue))/3;
          }
          else
            cost+=abs(refLight[face]-sensorLight[pos]);
        }
        if(cost<minCost)
        {
          minCost=cost;
          minCostTwist=twist;
        }
      }
      CostMatrixSet(corner,cubie,minCost);
      TwistMatrixSet(corner,cubie,minCostTwist);
    }
    for(int i=0;i<8;i++)
    {
      int cubie;
      int twist;
      int corner=BestFitCubie(cubie,twist,8);
      for(int s=0;s<3;s++)
      {
        int face = (CornerGet(corner,s))/9;
        int pos = CornerGet(cubie,(s+twist)%3);
        color[pos]=face;
        if(colorSensor)
        {
          //calibrate refColor
          rgb ref=refColor[face];
          rgb col=sensorColor[pos];
          ref.red+=col.red;
          ref.green+=col.green;
          ref.blue+=col.blue;
          refColor[face]=ref;
        }
        else
          refLight[face]+=sensorLight[pos]; //calibrate refLight
      }
    }
    if(colorSensor)
    {
      //calibrate refColor
      for(int face = 0; face < 6;face++)
      {
        rgb tmp=refColor[face];
        tmp.red/=5;
        tmp.green/=5;
        tmp.blue/=5;
        refColor[face]=tmp;
      }
    }
    else
      for(int face = 0; face < 6;face++)
        refLight[face]/=5; //calibrate refLight
}


void ResolveEdgeColors()
{
  TextOut(2,LCD_LINE6,"Edge colors...");
  for(int edge=0;edge<12;edge++)
    for(int cubie=0;cubie<12;cubie++)
    {
      unsigned int minCost=$FFFF;
      int minCostTwist;
      for(int twist=0;twist<2;twist++)
      {
        unsigned int cost=0;
        for(int s=0;s<2;s++)
        {
          int face = EdgeGet(edge,s)/ 9;
          int pos = EdgeGet(cubie,(s+twist)%2);
          if(colorSensor)
          {
            rgb color1,color2;
            color1 = refColor[face];
            color2 = sensorColor[pos];
            cost += ((color1.red - color2.red) * (color1.red - color2.red) + (color1.green - color2.green) * (color1.green - color2.green) + (color1.blue - color2.blue) * (color1.blue - color2.blue))/2;
          }
          else
            cost+=abs(refLight[face]-sensorLight[pos]);
        }
        if(cost<minCost)
        {
          minCost=cost;
          minCostTwist=twist;
        }
      }
      CostMatrixSet(edge,cubie,minCost);
      TwistMatrixSet(edge,cubie,minCostTwist);
    }
    for(int i = 0; i < 12;i++)
    {
      int cubie;
      int twist;
      int edge=BestFitCubie(cubie,twist,12);
      for(int s=0;s<2;s++)
        color[EdgeGet(cubie,(s+twist)%2)]=EdgeGet(edge,s)/9;
    }
}


void ScanCube()
{
  if(!colorSensor)
  {
    SetSensorColorFull(IN_2);
    SetSensorMode(IN_2,SENSOR_MODE_RAW);
  }
  OnFwd(OUT_C,COLOR_SENSOR_MOTOR_SLOW);
  until(SENSOR_1 == 0);
  Off(OUT_C);
  OnRev(OUT_C,COLOR_SENSOR_MOTOR_SLOW);
  until(SENSOR_1 == 1);
  Off(OUT_C);
  RotateMotor(OUT_C,COLOR_SENSOR_MOTOR_SLOW,OFFSETC);
  ScanFace(LEFTFACE,"LEFT FACE",0,0);
  Tilt();
  ScanFace(BACKFACE,"BACK FACE",1,0);
  Tilt();
  ScanFace(RIGHTFACE,"RIGHT FACE",1,3);
  Tilt();
  ScanFace(DOWNFACE,"DOWN FACE",1,1);
  Tilt();
  ScanFace(FRONTFACE,"FRONT FACE",0,1);
  Tilt();
  ScanFace(UPPERFACE,"UPPER FACE",0,0);
  SetSensorType(IN_2,SENSOR_TYPE_NONE);
  TextOut(2,LCD_LINE2,"RESOLVING COLORS",true);
  ResolveCenterColors();
  ResolveCornerColors();
  ResolveEdgeColors();
}


inline void CopyCube()
{
  ArraySubset(tmpCube,cube,0,54);
}


void CopyFace(int fromFace,int toFace)
{
  int fromFaceOffset=fromFace*9;
  int toFaceOffset=toFace*9;
  for(int i=0;i<9;i++)
    cube[toFaceOffset+i]=tmpCube[fromFaceOffset+i];
}


void CopyFaceClockwise(int fromFace, int toFace, int turns)
{
  int fromFaceOffset = fromFace*9;
  int toFaceOffset = toFace*9;
  if(turns==1)
  {
    cube[toFaceOffset + UPPERLEFT] = tmpCube[fromFaceOffset + DOWNLEFT];
    cube[toFaceOffset + UPPERMID] = tmpCube[fromFaceOffset + MIDLEFT];
    cube[toFaceOffset + UPPERRIGHT] = tmpCube[fromFaceOffset + UPPERLEFT];
    cube[toFaceOffset + MIDLEFT] = tmpCube[fromFaceOffset + DOWNMID];
    cube[toFaceOffset + CENTER] = tmpCube[fromFaceOffset + CENTER];
    cube[toFaceOffset + MIDRIGHT] = tmpCube[fromFaceOffset + UPPERMID];
    cube[toFaceOffset + DOWNLEFT] = tmpCube[fromFaceOffset + DOWNRIGHT];
    cube[toFaceOffset + DOWNMID] = tmpCube[fromFaceOffset + MIDRIGHT];
    cube[toFaceOffset + DOWNRIGHT] =tmpCube[fromFaceOffset +UPPERRIGHT];
  }
  else if(turns==2)
  {
    cube[toFaceOffset + UPPERLEFT] = tmpCube[fromFaceOffset + DOWNRIGHT];
    cube[toFaceOffset + UPPERMID] = tmpCube[fromFaceOffset + DOWNMID];
    cube[toFaceOffset + UPPERRIGHT] = tmpCube[fromFaceOffset + DOWNLEFT];
    cube[toFaceOffset + MIDLEFT] = tmpCube[fromFaceOffset + MIDRIGHT];
    cube[toFaceOffset + CENTER] = tmpCube[fromFaceOffset + CENTER];
    cube[toFaceOffset + MIDRIGHT] = tmpCube[fromFaceOffset + MIDLEFT];
    cube[toFaceOffset + DOWNLEFT] = tmpCube[fromFaceOffset + UPPERRIGHT];
    cube[toFaceOffset + DOWNMID] = tmpCube[fromFaceOffset + UPPERMID];
    cube[toFaceOffset + DOWNRIGHT] =tmpCube[fromFaceOffset +UPPERLEFT];
  }
  else //turns==3
  {
    cube[toFaceOffset + UPPERLEFT] = tmpCube[fromFaceOffset + UPPERRIGHT];
    cube[toFaceOffset + UPPERMID] = tmpCube[fromFaceOffset + MIDRIGHT];
    cube[toFaceOffset + UPPERRIGHT] = tmpCube[fromFaceOffset + DOWNRIGHT];
    cube[toFaceOffset + MIDLEFT] = tmpCube[fromFaceOffset + UPPERMID];
    cube[toFaceOffset + CENTER] = tmpCube[fromFaceOffset + CENTER];
    cube[toFaceOffset + MIDRIGHT] = tmpCube[fromFaceOffset + DOWNMID];
    cube[toFaceOffset + DOWNLEFT] = tmpCube[fromFaceOffset + UPPERLEFT];
    cube[toFaceOffset + DOWNMID] = tmpCube[fromFaceOffset + MIDLEFT];
    cube[toFaceOffset + DOWNRIGHT] =tmpCube[fromFaceOffset +DOWNLEFT];
  }
}


void TurnCube(int turns)
{
  CopyCube();
  if(turns==1)
  {
    CopyFaceClockwise(UPPERFACE,UPPERFACE,1);
    CopyFace(LEFTFACE,BACKFACE);
    CopyFace(BACKFACE,RIGHTFACE);
    CopyFace(RIGHTFACE,FRONTFACE);
    CopyFace(FRONTFACE,LEFTFACE);
    CopyFaceClockwise(DOWNFACE,DOWNFACE,3);
  }
  else if(turns==2)
  {
    CopyFaceClockwise(UPPERFACE,UPPERFACE,2);
    CopyFace(LEFTFACE,RIGHTFACE);
    CopyFace(BACKFACE,FRONTFACE);
    CopyFace(RIGHTFACE,LEFTFACE);
    CopyFace(FRONTFACE,BACKFACE);
    CopyFaceClockwise(DOWNFACE,DOWNFACE,2);
  }
  else //turns==3
  {
    CopyFaceClockwise(UPPERFACE,UPPERFACE,3);
    CopyFace(LEFTFACE,FRONTFACE);
    CopyFace(BACKFACE,LEFTFACE);
    CopyFace(RIGHTFACE,BACKFACE);
    CopyFace(FRONTFACE,RIGHTFACE);
    CopyFaceClockwise(DOWNFACE,DOWNFACE,1);
  }
}


void TiltCube(int turns)
{
  CopyCube();
  if(turns==1)
  {
    CopyFaceClockwise(UPPERFACE,RIGHTFACE,1);
    CopyFaceClockwise(RIGHTFACE,DOWNFACE,1);
    CopyFaceClockwise(DOWNFACE,LEFTFACE,1);
    CopyFaceClockwise(LEFTFACE,UPPERFACE,1);
    CopyFaceClockwise(FRONTFACE,FRONTFACE,1);
    CopyFaceClockwise(BACKFACE,BACKFACE,3);
  }
  else if(turns==2)
  {
    CopyFaceClockwise(UPPERFACE,DOWNFACE,2);
    CopyFaceClockwise(RIGHTFACE,LEFTFACE,2);
    CopyFaceClockwise(DOWNFACE,UPPERFACE,2);
    CopyFaceClockwise(LEFTFACE,RIGHTFACE,2);
    CopyFaceClockwise(FRONTFACE,FRONTFACE,2);
    CopyFaceClockwise(BACKFACE,BACKFACE,2);
  }
  else //turns==3
  {
    CopyFaceClockwise(UPPERFACE,LEFTFACE,3);
    CopyFaceClockwise(RIGHTFACE,UPPERFACE,3);
    CopyFaceClockwise(DOWNFACE,RIGHTFACE,3);
    CopyFaceClockwise(LEFTFACE,DOWNFACE,3);
    CopyFaceClockwise(FRONTFACE,FRONTFACE,3);
    CopyFaceClockwise(BACKFACE,BACKFACE,1);
  }
}


void TwistCube(int turns)
{
  char move;
  for (int twists = 0; twists < turns; twists++)
  {
    CopyCube();
    CopyFaceClockwise(DOWNFACE, DOWNFACE,1);
    for (int i = 6; i < 9; i++)
    {
      CubeSet(LEFTFACE, i, TmpCubeGet(BACKFACE, i));
      CubeSet(FRONTFACE, i, TmpCubeGet(LEFTFACE, i));
      CubeSet(RIGHTFACE, i, TmpCubeGet(FRONTFACE, i));
      CubeSet(BACKFACE, i, TmpCubeGet(RIGHTFACE, i));
    }
    move=cube[DOWNFACE_CENTER];
    moves[movesCount++]=move;
  }
  PlayTone(200+movesCount*20,1);
}


void RotateFace(int face, int turns)
{
  switch (face)
  {
    case UPPERFACE:
      TiltCube(2); TwistCube(turns); TiltCube(2); break;
    case LEFTFACE:
      TiltCube(3); TwistCube(turns); TiltCube(1); break;
    case FRONTFACE:
      TurnCube(1); TiltCube(3); TwistCube(turns); TiltCube(1); TurnCube(3); break;
    case RIGHTFACE:
      TiltCube(1); TwistCube(turns); TiltCube(3); break;
    case BACKFACE:
      TurnCube(3); TiltCube(3); TwistCube(turns); TiltCube(1); TurnCube(1); break;
    case DOWNFACE:
      TwistCube(turns); break;
  }
}


void RotateFaces(string faces)
{
  char faceturn;
  for (int i = 0; i < StrLen(faces); i++)
  {
    faceturn=faces[i];
    switch (faceturn)
    {
      case 'U': RotateFace(UPPERFACE, 1); break;
      case 'L': RotateFace(LEFTFACE, 1); break;
      case 'F': RotateFace(FRONTFACE, 1); break;
      case 'R': RotateFace(RIGHTFACE, 1); break;
      case 'B': RotateFace(BACKFACE, 1); break;
      case 'D': RotateFace(DOWNFACE, 1); break;
      case 'u': RotateFace(UPPERFACE, 3); break;
      case 'l': RotateFace(LEFTFACE, 3); break;
      case 'f': RotateFace(FRONTFACE, 3); break;
      case 'r': RotateFace(RIGHTFACE, 3); break;
      case 'b': RotateFace(BACKFACE, 3); break;
      case 'd': RotateFace(DOWNFACE, 3); break;
    }
  }
}


bool CornerColorOk(int position, char c1, char c2)
{
  return cube[position] == c1 || cube[position] == c2;
}


bool TryBottomFace(char c1, char c2, int twists)
{
  for (int i = 0; i < 4; i++)
  {
    if (twists == 0)
    {
      if (CornerColorOk(DOWNFACE_UPPERLEFT, c1, c2) && CornerColorOk(DOWNFACE_UPPERRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && !CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2))
        return true;
    }
    else if (twists == 1)
    {
      if (CornerColorOk(DOWNFACE_UPPERLEFT, c1, c2) && CornerColorOk(DOWNFACE_UPPERRIGHT, c1, c2))
      {
        for (int j = 0; j < 4; j++)
        {
          RotateFaces("B");
          if (CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && !CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2))
            return true;
          else if (!CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2))
          {
            TurnCube(3);
            return true;
          }
          else if (CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2))
          {
            if (CornerColorOk(UPPERFACE_UPPERLEFT, c1, c2) && CornerColorOk(UPPERFACE_UPPERRIGHT, c1, c2) && CornerColorOk(UPPERFACE_DOWNLEFT, c1, c2) && CornerColorOk(UPPERFACE_DOWNRIGHT, c1, c2))
              return true;
          }
        }
      }
    }
    else if (twists == 2)
    {
      if (CornerColorOk(DOWNFACE_UPPERLEFT, c1, c2))
      {
        for (int j = 0; j < 4; j++)
        {
          RotateFaces("R");
          if (CornerColorOk(DOWNFACE_UPPERRIGHT, c1, c2))
          {
            for (int k = 0; k < 4; k++)
            {
              RotateFaces("B");
              if (CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && !CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2))
                return true;
              else if (!CornerColorOk(DOWNFACE_DOWNRIGHT, c1, c2) && CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2))
              {
                TurnCube(3);
                return true;
              }
            }
          }
        }
      }
    }
    TurnCube(1);
  }
  return false;
}


bool PrepareBottomFace(char c1, char c2, int twists)
{
  if (TryBottomFace(c1, c2, twists))
    return true;
  TiltCube(1);
  if (TryBottomFace(c1, c2, twists))
    return true;
  TiltCube(1);
  if (TryBottomFace(c1, c2, twists))
    return true;
  TiltCube(1);
  if (TryBottomFace(c1, c2, twists))
    return true;
  TurnCube(1);
  TiltCube(1);
  if (TryBottomFace(c1, c2, twists))
    return true;
  TiltCube(2);
  if (TryBottomFace(c1, c2, twists))
    return true;
  return false;
}


bool MoveCorners(int corner1,int corner2,int corner3,int corner4, char color1, char color2, string moves)
{
  if (CornerColorOk(corner1, color1, color2) &&
    CornerColorOk(corner2, color1, color2) &&
    CornerColorOk(corner3, color1, color2) &&
    CornerColorOk(corner4, color1, color2))
  {
    RotateFaces(moves);
    return true;
  }
  return false;
}


void OrientAllCorners(char c1, char c2)
{
  if (!PrepareBottomFace(c1, c2, 0))
    if (!PrepareBottomFace(c1, c2, 1))
      PrepareBottomFace(c1, c2, 2);
  if (CornerColorOk(DOWNFACE_DOWNLEFT, c1, c2))
    return;
  else if (CornerColorOk(LEFTFACE_DOWNLEFT, c1, c2))
  {
    for (int i = 0; i < 4; i++)
    {
      if (MoveCorners(BACKFACE_UPPERRIGHT, RIGHTFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "Lul"))
        return;
      else if (MoveCorners(BACKFACE_UPPERRIGHT, UPPERFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "flF"))
        return;
      else if (MoveCorners(LEFTFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, LEFTFACE_UPPERRIGHT, RIGHTFACE_UPPERLEFT, c1, c2, "fLLF"))
        return;
      else if (MoveCorners(BACKFACE_UPPERRIGHT, UPPERFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "LLDF"))
        return;
      else if (MoveCorners(UPPERFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "LfLf"))
        return;
      else if (MoveCorners(LEFTFACE_UPPERLEFT, BACKFACE_UPPERLEFT, LEFTFACE_UPPERRIGHT, UPPERFACE_DOWNRIGHT, c1, c2, "bDDLdl"))
        return;
      else if (MoveCorners(BACKFACE_UPPERRIGHT, RIGHTFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "fLfDDb"))
        return;
      else if (MoveCorners(LEFTFACE_UPPERLEFT, BACKFACE_UPPERLEFT, FRONTFACE_UPPERLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "lULfLLF"))
        return;
      RotateFaces("U");
    }
  }
  else if (CornerColorOk(BACKFACE_DOWNRIGHT, c1, c2))
  {
    for (int i = 0; i < 4; i++)
    {
      if (MoveCorners(LEFTFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "bUB"))
        return;
      else if (MoveCorners(LEFTFACE_UPPERLEFT, UPPERFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "LDF"))
        return;
      else if (MoveCorners(BACKFACE_UPPERRIGHT, BACKFACE_UPPERLEFT, UPPERFACE_DOWNLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "RBBr"))
        return;
      else if (MoveCorners(LEFTFACE_UPPERLEFT, RIGHTFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, FRONTFACE_UPPERRIGHT, c1, c2, "FFdB"))
        return;
      else if (MoveCorners(UPPERFACE_UPPERLEFT, RIGHTFACE_UPPERRIGHT, UPPERFACE_DOWNLEFT, UPPERFACE_DOWNRIGHT, c1, c2, "bRbr"))
        return;
      else if (MoveCorners(BACKFACE_UPPERRIGHT, BACKFACE_UPPERLEFT, LEFTFACE_UPPERRIGHT, UPPERFACE_DOWNRIGHT, c1, c2, "LUUfDF"))
        return;
      else if (MoveCorners(LEFTFACE_UPPERLEFT, RIGHTFACE_UPPERRIGHT, FRONTFACE_UPPERLEFT, RIGHTFACE_UPPERLEFT, c1, c2, "RbRDDL"))
        return;
      else if (MoveCorners(BACKFACE_UPPERRIGHT, RIGHTFACE_UPPERRIGHT, LEFTFACE_UPPERRIGHT, FRONTFACE_UPPERRIGHT, c1, c2, "FluRUUR"))
        return;
      RotateFaces("U");
    }
  }
}


void SplitCorners(char color, char oppositeColor)
{
  int count = 0;
  for (int i = 0; i < 4; i++)
  {
    if (cube[DOWNFACE_UPPERLEFT] == color)
      count++;
    TurnCube(1);
  }
  if (count == 1 || count == 3)
  {
    int singleColor;
    if (count == 1)
      singleColor = color;
    else
      singleColor = oppositeColor;
    while (cube[DOWNFACE_UPPERLEFT] != singleColor)
      TurnCube(1);
    while (cube[UPPERFACE_DOWNRIGHT] == singleColor)
      RotateFaces("U");
    if (cube[UPPERFACE_DOWNRIGHT] == cube[UPPERFACE_CENTER])
      RotateFaces("RRDLL");
    else
      RotateFaces("RRDRR");
  }
  else if (count == 2)
  {
    if (cube[DOWNFACE_UPPERLEFT] != cube[DOWNFACE_DOWNRIGHT])
      TiltCube(2);
    if (cube[DOWNFACE_UPPERLEFT] != cube[DOWNFACE_DOWNRIGHT])
    {
      while (cube[DOWNFACE_UPPERLEFT] != cube[DOWNFACE_UPPERRIGHT])
        TurnCube(1);
      while (cube[UPPERFACE_UPPERLEFT] != cube[DOWNFACE_UPPERLEFT] || cube[UPPERFACE_UPPERRIGHT] != cube[DOWNFACE_UPPERRIGHT])
        RotateFaces("U");
      if (cube[UPPERFACE_UPPERLEFT] == cube[UPPERFACE_CENTER])
        RotateFaces("FF");
      else
        RotateFaces("BB");
    }
    else if (cube[UPPERFACE_UPPERLEFT] == cube[UPPERFACE_DOWNRIGHT])
    {
      if (cube[UPPERFACE_UPPERLEFT] != cube[DOWNFACE_DOWNLEFT])
        RotateFaces("U");
      if (cube[UPPERFACE_UPPERRIGHT] == cube[UPPERFACE_CENTER])
        TurnCube(1);
      RotateFaces("RRDDFF");
    }
    else
    {
      while (cube[UPPERFACE_UPPERLEFT] != cube[DOWNFACE_DOWNLEFT] || cube[UPPERFACE_DOWNLEFT] != cube[DOWNFACE_DOWNLEFT])
        TurnCube(1);
      if (cube[UPPERFACE_UPPERLEFT] != cube[UPPERFACE_CENTER])
        RotateFaces("RRDRRDLL");
      else
        RotateFaces("RRDRRDRR");
    }
  }
  if (cube[UPPERFACE_UPPERLEFT] == cube[LEFTFACE_CENTER])
    TiltCube(1);
  else if (cube[UPPERFACE_UPPERLEFT] == cube[FRONTFACE_CENTER])
  {
    TurnCube(1);
    TiltCube(1);
  }
  else if (cube[UPPERFACE_UPPERLEFT] == cube[RIGHTFACE_CENTER])
    TiltCube(3);
  else if (cube[UPPERFACE_UPPERLEFT] == cube[BACKFACE_CENTER])
  {
    TurnCube(3);
    TiltCube(1);
  }
  else if (cube[UPPERFACE_UPPERLEFT] == cube[DOWNFACE_CENTER])
    TiltCube(2);
  while (cube[UPPERFACE_UPPERLEFT] != cube[UPPERFACE_CENTER])
    RotateFaces("B");
  while (cube[UPPERFACE_DOWNLEFT] != cube[UPPERFACE_CENTER])
    RotateFaces("F");
}


//Step 3 Position all corners
void PositionAllCorners()
{
  int count = 0;
  int topCount = 0;
  int bottomCount = 0;
  for (int i = 0; i < 4; i++)
  {
    if (cube[BACKFACE_DOWNLEFT] == cube[BACKFACE_DOWNRIGHT])
      bottomCount++;
    if (cube[BACKFACE_UPPERLEFT] == cube[BACKFACE_UPPERRIGHT])
      topCount++;
    TurnCube(1);
  }
  if (topCount > bottomCount)
    TiltCube(2);
  count = topCount + bottomCount;
  if (count == 0)
    RotateFaces("RRFFRR");
  else if (count == 1)
  {
    while (cube[BACKFACE_DOWNLEFT] != cube[BACKFACE_DOWNRIGHT])
      TurnCube(1);
    RotateFaces("RuFUUfUr");
  }
  else if (count == 2)
  {
    while (cube[BACKFACE_DOWNLEFT] != cube[BACKFACE_DOWNRIGHT])
      TurnCube(1);
    while (cube[BACKFACE_UPPERLEFT] != cube[BACKFACE_UPPERRIGHT])
      RotateFaces("U");
    RotateFaces("RRUFFUURRURR");
  }
  else if (count == 4)
    RotateFaces("FFuRurUFFURUr");
  else if (count == 5)
  {
    while (cube[BACKFACE_UPPERLEFT] != cube[BACKFACE_UPPERRIGHT])
      TurnCube(1);
    RotateFaces("RuRFFrURFFRR");
  }
}


int TopEdgesSolved()
{
  int solved = 0;
  if (cube[UPPERFACE_UPPERMID] == cube[UPPERFACE_CENTER] && cube[BACKFACE_UPPERMID] == cube[BACKFACE_UPPERLEFT])
    solved++;
  if (cube[UPPERFACE_MIDLEFT] == cube[UPPERFACE_CENTER] && cube[LEFTFACE_UPPERMID] == cube[LEFTFACE_UPPERLEFT])
    solved++;
  if (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[RIGHTFACE_UPPERMID] == cube[RIGHTFACE_UPPERLEFT])
    solved++;
  if (cube[UPPERFACE_DOWNMID] == cube[UPPERFACE_CENTER] && cube[FRONTFACE_UPPERMID] == cube[FRONTFACE_UPPERLEFT])
    solved++;
  return solved;
}


int BottomEdgesSolved()
{
  int solved = 0;
  if (cube[DOWNFACE_UPPERMID] == cube[DOWNFACE_CENTER] && cube[FRONTFACE_DOWNMID] == cube[FRONTFACE_DOWNLEFT])
    solved++;
  if (cube[DOWNFACE_MIDLEFT] == cube[DOWNFACE_CENTER] && cube[LEFTFACE_DOWNMID] == cube[LEFTFACE_DOWNLEFT])
    solved++;
  if (cube[DOWNFACE_MIDRIGHT] == cube[DOWNFACE_CENTER] && cube[RIGHTFACE_DOWNMID] == cube[RIGHTFACE_DOWNLEFT])
    solved++;
  if (cube[DOWNFACE_DOWNMID] == cube[DOWNFACE_CENTER] && cube[BACKFACE_DOWNMID] == cube[BACKFACE_DOWNLEFT])
    solved++;
  return solved;
}


void SetBottomFace(int downface, int downpos, int sideface, int sidepos)
{
  if (CubeGet(downface, downpos) == cube[DOWNFACE_CENTER])
  {
    while (cube[RIGHTFACE_DOWNLEFT] != CubeGet(sideface, sidepos))
      RotateFaces("D");
  }
  else
  {
    for (int i = 0; i < 4; i++)
    {
      if (cube[DOWNFACE_MIDRIGHT] != cube[DOWNFACE_CENTER] || cube[RIGHTFACE_DOWNMID] != cube[RIGHTFACE_DOWNLEFT])
        break;
      RotateFaces("D");
    }
  }
}


void TopEdgeMoveOut()
{
  for (int i = 0; i < 4; i++)
  {
    if (cube[UPPERFACE_MIDRIGHT] != cube[UPPERFACE_CENTER] || cube[RIGHTFACE_UPPERMID] != cube[RIGHTFACE_UPPERLEFT])
    {
      SetBottomFace(0, 0, 0, 0);
      RotateFaces("rUdF");
      return;
    }
    TurnCube(1);
  }
}


bool TopEdgeShort()
{
  for (int i = 0; i < 4; i++)
  {
    for (int j = 0; j < 4; j++)
    {
      if (cube[LEFTFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[FRONTFACE_MIDLEFT] == cube[RIGHTFACE_UPPERLEFT])
      {
        SetBottomFace(RIGHTFACE, UPPERMID, UPPERFACE, MIDRIGHT);
        RotateFaces("rUdF");
        return true;
      }
      if (cube[LEFTFACE_MIDLEFT] == cube[UPPERFACE_CENTER] && cube[BACKFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT])
      {
        SetBottomFace(RIGHTFACE, UPPERMID, UPPERFACE, MIDRIGHT);
        RotateFaces("RuDb");
        return true;
      }
      if (cube[FRONTFACE_MIDLEFT] == cube[UPPERFACE_CENTER] && cube[LEFTFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT])
      {
        SetBottomFace(BACKFACE, MIDRIGHT, LEFTFACE, MIDLEFT);
        RotateFaces("RUUddl");
        return true;
      }
      if (cube[BACKFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[LEFTFACE_MIDLEFT] == cube[RIGHTFACE_UPPERLEFT])
      {
        SetBottomFace(FRONTFACE, MIDLEFT, LEFTFACE, MIDRIGHT);
        RotateFaces("ruuDDL");
        return true;
      }
      if (cube[RIGHTFACE_DOWNMID] == cube[UPPERFACE_CENTER] && cube[DOWNFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT])
      {
        RotateFaces("RUdf");
        return true;
      }
      TurnCube(1);
    }
    RotateFaces("U");
  }
  return false;
}


bool TopEdgeLong()
{
  for (int i = 0; i < 4; i++)
  {
    for (int j = 0; j < 4; j++)
    {
      if (cube[DOWNFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] && cube[RIGHTFACE_DOWNMID] == cube[RIGHTFACE_UPPERLEFT])
      {
        SetBottomFace(BACKFACE, MIDRIGHT, LEFTFACE, MIDLEFT);
        RotateFaces("RuDBBUdR");
        return true;
      }
      if (cube[RIGHTFACE_UPPERMID] == cube[UPPERFACE_CENTER] && cube[UPPERFACE_MIDRIGHT] == cube[RIGHTFACE_UPPERLEFT])
      {
        SetBottomFace(LEFTFACE, MIDRIGHT, FRONTFACE, MIDLEFT);
        RotateFaces("ruDBBuuDDf");
        return true;
      }
      TurnCube(1);
    }
    RotateFaces("U");
  }
  return false;
}


//Step 4 Solve top and bottom edges
void SolveTopAndBottomEdges()
{
  int topEdgesSolved;
  int bottomEdgesSolved;
  while (true)
  {
    topEdgesSolved = TopEdgesSolved();
    bottomEdgesSolved = BottomEdgesSolved();
    if (topEdgesSolved + bottomEdgesSolved >= 7)
        break;
    if (topEdgesSolved < 3 || bottomEdgesSolved == 3)
      if (TopEdgeShort())
        continue;
    if (bottomEdgesSolved < 3 || topEdgesSolved == 3)
    {
      TiltCube(2);
      if (TopEdgeShort())
        continue;
    }
    if (topEdgesSolved < 3 || bottomEdgesSolved == 3)
      if (TopEdgeLong())
        continue;
    if (bottomEdgesSolved < 3 || topEdgesSolved == 3)
    {
      TiltCube(2);
      if (TopEdgeLong())
        continue;
    }
    if (topEdgesSolved >= 3)
      TiltCube(2);
    TopEdgeMoveOut();  //If two edges are swapped on upperface
  }
  if (bottomEdgesSolved < 4)
    TiltCube(2);
}


void PrepareMiddleEdges()
{
  for (int i = 0; i < 4; i++)
  {
    if (cube[LEFTFACE_MIDRIGHT] == cube[UPPERFACE_CENTER] || cube[LEFTFACE_MIDLEFT] == cube[UPPERFACE_CENTER])
    {
      while (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER])
        RotateFaces("U");
      break;
    }
    TurnCube(1);
  }
  for (int i = 0; i < 4; i++)
  {
    if (cube[UPPERFACE_MIDRIGHT] != cube[UPPERFACE_CENTER] || cube[RIGHTFACE_UPPERMID] != cube[RIGHTFACE_UPPERLEFT])
      break;
    TurnCube(1);
  }
}


int TopEdgesInMiddleLayerOrientation()
{
  int orientation = 0;
  if (cube[RIGHTFACE_MIDLEFT] != cube[LEFTFACE_CENTER] && cube[RIGHTFACE_MIDLEFT] != cube[RIGHTFACE_CENTER])
    orientation = 4;
  if (cube[RIGHTFACE_UPPERMID] != cube[LEFTFACE_CENTER] && cube[RIGHTFACE_UPPERMID] != cube[RIGHTFACE_CENTER])
    orientation += 2;
  if (cube[RIGHTFACE_MIDRIGHT] != cube[LEFTFACE_CENTER] && cube[RIGHTFACE_MIDRIGHT] != cube[RIGHTFACE_CENTER])
    orientation += 1;
  return orientation;
}


bool MiddleEdgeTwisted(int face, int pos)
{
  return CubeGet(face, pos) != cube[FRONTFACE_CENTER] && CubeGet(face, pos) != cube[BACKFACE_CENTER];
}


int TwistedMiddleEdges()
{
  int twisted = 0;
  for (int i = 0; i < 4; i++)
  {
    if (MiddleEdgeTwisted(FRONTFACE, MIDRIGHT))
      twisted++;
    TurnCube(1);
  }
  return twisted;
}


//Step 5 Orient middle edges
void OrientMiddleEdges()
{
  PrepareMiddleEdges();
  if (cube[LEFTFACE_MIDRIGHT] == cube[UPPERFACE_CENTER])
  {
    switch (TopEdgesInMiddleLayerOrientation())
    {
      case 0: //OOO
        RotateFaces("UdFUdluDfUdL"); break;
      case 1: //OOX
        RotateFaces("UdfuDrUdfuDr"); break;
      case 2: //OXO
        RotateFaces("uDBUdRRUdF"); break;
      case 3: //OXX
        RotateFaces("ruDBUdrUdF"); break;
      case 4: //XOO
        RotateFaces("RRUdFuDRUdFuDr"); break;
      case 5: //XOX
        RotateFaces("rUdfuDruDBUdRuDB"); break;
      case 6: //XXO
        RotateFaces("ruDbUdRUdF"); break;
      case 7: //XXX
        RotateFaces("rUdfUdluDFFuDR"); break;
    }
  }
  else if (cube[LEFTFACE_MIDLEFT] == cube[UPPERFACE_CENTER])
  {
    switch (TopEdgesInMiddleLayerOrientation())
    {
      case 0: //OOO
        RotateFaces("uDbuDLUdBuDl"); break;
      case 1: //OOX
        RotateFaces("rruDbUdruDbUdR"); break;
      case 2: //OXO
        RotateFaces("UdfuDrruDb"); break;
      case 3: //OXX
        RotateFaces("RUdFuDruDb"); break;
      case 4: //XOO
        RotateFaces("uDBUdRuDBUdR"); break;
      case 5: //XOX
        RotateFaces("RuDBUdRUdfuDrUdf"); break;
      case 6: //XXO
        RotateFaces("RUdfuDRuDb"); break;
      case 7: //XXX
        RotateFaces("RuDBuDLUdbbUdr"); break;
    }
  }
  else if (cube[RIGHTFACE_UPPERMID] == cube[UPPERFACE_CENTER])
  {
    switch (TwistedMiddleEdges())
    {
      case 1:
        while (!MiddleEdgeTwisted(FRONTFACE, MIDRIGHT))
          TurnCube(1);
        while(cube[UPPERFACE_MIDLEFT]==cube[UPPERFACE_CENTER])
          RotateFaces("U");
        RotateFaces("RUUrUUddLLuDfUUf");
        break;
      case 3:
        while (MiddleEdgeTwisted(FRONTFACE, MIDRIGHT))
          TurnCube(1);
        while (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER])
          RotateFaces("U");
        RotateFaces("ruDbuDluDf");
        break;
    }
  }
  else if (cube[UPPERFACE_MIDRIGHT] == cube[UPPERFACE_CENTER])
  {
    switch (TwistedMiddleEdges())
    {
      case 2:
        while (true)
        {
          if (MiddleEdgeTwisted(FRONTFACE, MIDLEFT) && MiddleEdgeTwisted(FRONTFACE, MIDRIGHT))
          {
            RotateFaces("RRFlRuRRULrf");
            return;
          }
          else if (MiddleEdgeTwisted(FRONTFACE, MIDLEFT) && MiddleEdgeTwisted(BACKFACE, MIDLEFT))
          {
            RotateFaces("FlRuRRULrfRR");
            return;
          }
          TurnCube(1);
        }
      case 4:
        RotateFaces("RFFRRUdFUUddBRRBBUdR");
        break;
    }
  }
}


bool Rotate3MiddleEdges()
{
  for (int i = 0; i < 2; i++)
  {
    for (int j = 0; j < 4; j++)
    {
      if (cube[LEFTFACE_MIDRIGHT] == cube[LEFTFACE_CENTER] && cube[FRONTFACE_MIDLEFT] == cube[FRONTFACE_CENTER] &&
        cube[FRONTFACE_MIDRIGHT] == cube[BACKFACE_CENTER] && cube[RIGHTFACE_MIDLEFT] == cube[LEFTFACE_CENTER] &&
        cube[BACKFACE_MIDRIGHT] == cube[BACKFACE_CENTER] && cube[LEFTFACE_MIDLEFT] == cube[RIGHTFACE_CENTER])
      {
        RotateFaces("RRuDBB");
        return true;
      }
      TurnCube(1);
    }
    TiltCube(2);
  }
  return false;
}


bool ExchangeMiddleCenters()
{
  bool exchangeCenters = true;
  for (int i = 0; i < 4; i++)
  {
    if ((cube[FRONTFACE_CENTER] != cube[BACKFACE_MIDLEFT]) || (cube[FRONTFACE_CENTER] != cube[BACKFACE_MIDRIGHT]))
    {
      exchangeCenters = false;
      break;
    }
    TurnCube(1);
  }
  if (exchangeCenters)
    RotateFaces("LLRRuDFFBB");
  return exchangeCenters;
}


bool ExchangeMiddleCorners()
{
  for (int i = 0; i < 2; i++)
  {
    if (cube[FRONTFACE_MIDLEFT] == cube[BACKFACE_CENTER] && cube[FRONTFACE_MIDRIGHT] == cube[BACKFACE_CENTER] &&
      cube[BACKFACE_MIDLEFT] == cube[FRONTFACE_CENTER] && cube[BACKFACE_MIDRIGHT] == cube[FRONTFACE_CENTER] &&
      cube[LEFTFACE_MIDLEFT] == cube[LEFTFACE_CENTER] && cube[LEFTFACE_MIDRIGHT] == cube[LEFTFACE_CENTER])
    {
      RotateFaces("RRUUddLL");
      return true;
    }
    TurnCube(1);
  }
  return false;
}


//Step 6 Position middle edges
void PositionMiddleEdges()
{
  if (!Rotate3MiddleEdges())
    if (!ExchangeMiddleCenters())
      ExchangeMiddleCorners();
  while (cube[FRONTFACE_UPPERMID] != cube[FRONTFACE_CENTER])
    RotateFaces("U");
  while (cube[FRONTFACE_DOWNMID] != cube[FRONTFACE_CENTER])
    RotateFaces("D");
}


void Optimize()
{
  char move;
  int count;
  int pos;
  int optcount;
  optcount=movesCount;
  do
  {
    twists=0;
    movesCount=optcount;
    moves[movesCount]=0;
    optcount=0;
    pos=0;
    while(pos < movesCount)
    {
      move = moves[pos];
      count = 1;
      while (moves[++pos] == move)
        count++;
      count = count % 4;
      for(int i=0;i<count;i++)
        moves[optcount++]=move;
      twists++;
    }
  }while(optcount < movesCount);
  movesCount=optcount;
  moves[movesCount]=0;
}


char FaceFind(char face)
{
  for(int i=0;i<6;i++)
    if(faces[i] == face)
      return staticfaces[i];
}


void FaceDown(char face)
{
   switch(FaceFind(face))
  {
    case 'U': Tilt();Tilt();break;
    case 'L': Turn(2);Tilt();break;
    case 'F': Turn(1);Tilt();break;
    case 'R': Tilt();break;
    case 'B': Turn(3);Tilt();break;
  }
}


void DoMoves()
{
  int turns;
  int pos=0;
  char face;
  for(int i=0;i<6;i++)
    faces[i]=staticfaces[i];

  while(pos < solutionCount)
  {
    face=solution[pos];
    FaceDown(face);
    turns=1;
    while(solution[++pos]==face)
      turns++;
    Twist(turns);
    NumOut(40,LCD_LINE4,--solutionTwists,true);
  }
  Turn(4);
  TextOut(20,LCD_LINE4,"GAME OVER",true);
  PlayFile("Game Over.rso");
  Wait(3000);
}


void SetCubeFace(int c,char f)
{
  for(int pos=0;pos<6*9;pos++)
    if(color[pos]==c)
      cube[pos]=f;
}


void LoadCube()
{
  SetCubeFace(UPPERFACE,'U');
  SetCubeFace(LEFTFACE,'L');
  SetCubeFace(FRONTFACE,'F');
  SetCubeFace(RIGHTFACE,'R');
  SetCubeFace(BACKFACE,'B');
  SetCubeFace(DOWNFACE,'D');
  for(int i=0;i<6;i++)
    faces[i]=staticfaces[i];
  movesCount=0;
}


void SaveSolution()
{
  for(int i=0;i<movesCount;i++)
    solution[i]=moves[i];
  solutionCount=movesCount;
  solutionTwists=twists;
}


void Solve(char color, char oppositeColor )
{
  OrientAllCorners(color, oppositeColor);
  SplitCorners(color, oppositeColor);
  PositionAllCorners();
  SolveTopAndBottomEdges();
  OrientMiddleEdges();
  PositionMiddleEdges();
  Optimize();
}


void SolveCube()
{
  TextOut(20,LCD_LINE2,"SOLVING",true);
  TextOut(0,LCD_LINE4,"Solution 1 =");
  LoadCube();
  Solve('U','D');
  NumOut (80, LCD_LINE4, twists);
  SaveSolution();

  TextOut(0,LCD_LINE5,"Solution 2 =");
  LoadCube();
  Solve('F','B');
  NumOut (80, LCD_LINE5, twists);
  if(twists < solutionTwists)
    SaveSolution();

  TextOut(0,LCD_LINE6,"Solution 3 =");
  LoadCube();
  Solve('L','R');
  NumOut (80, LCD_LINE6, twists);
  Wait(500);
  if(twists < solutionTwists)
    SaveSolution();
}


void WaitForCube()
{
  TextOut(0,LCD_LINE5,"GIVE ME A CUBE !");
  Wait(1000);
  SensorUS(IN_3);
  while(true)
  {
    if(SensorUS(IN_3) < 10)
    {
      Wait(1000);
      if(SensorUS(IN_3) < 10)
        break;
    }
  }
  TextOut(20,LCD_LINE5,"THANK YOU !",true);
  PlayFile("Thank You.rso");
  Wait(2000);
}


void RefColorSet(int face,int r, int g, int b)
{
  rgb col;
  col.red=r;
  col.green=g;
  col.blue=b;
  refColor[face]=col;

}


void LoadCalibration()
{
  int handle;
  int dummy;
  tachoCenter = 150;  //default
  if(colorSensor)
  {
    if(OpenFileRead("tiltedtwister.clr",dummy,handle)==0)
    {
      Read(handle,tachoCenter);
      for(int face=0;face<6;face++)
      {
        int r,g,b;
        Read(handle,r);
        Read(handle,g);
        Read(handle,b);
        RefColorSet(face,r,g,b);
       NumOut (0,LCD_LINE1, r);
       NumOut (0,LCD_LINE2, g);
       NumOut (0,LCD_LINE3, b);
       NumOut (0,LCD_LINE4, face);
       Wait (800);
      }
      CloseFile(handle);
    }
    else
    {
      //Values of my standard Rubik's cube
      RefColorSet(LEFTFACE,2,2,2);       //blue
      RefColorSet(FRONTFACE,1,1,1);    //orange
      RefColorSet(RIGHTFACE,3,3,3);      //green
      RefColorSet(BACKFACE,5,5,5);       //red
      RefColorSet(UPPERFACE,6,6,6); //white
      RefColorSet(DOWNFACE,4,4,4);    //yellow
    }
  }
  else  //lightsensor
  {
    if(OpenFileRead("tiltedtwister.lgt",dummy,handle)==0)
    {
      Read(handle,tachoCenter);
      CloseFile(handle);
    }
  }
}


void Initialize()
{
  SetSensorColorFull(IN_2);
  SetSensorMode(IN_2,SENSOR_MODE_RAW);
  colorSensor = true;
  SetSensorColorFull(IN_2);
  LoadCalibration();
  if(colorSensor)
  {
    SetSensorColorFull(IN_2);
    Wait(100);
    TextOut(0,LCD_LINE1,"color sensor");
  }
  else
  {
  TextOut(0,LCD_LINE1,"light sensor");
  }
  SetSensorLowspeed(IN_3);
  SetSensorTouch(IN_1);
}


task main()
{
  Initialize();
  WaitForCube();
  
/*   Actuator test
  while(true)
  {
    Twist(1);
    Wait(1000);
    Tilt();
    Wait(1000);
    Twist(3);
    Wait(1000);
    Tilt();
    Wait(1000);
  }
*/

  ScanCube();
  SolveCube();
  DoMoves();
}

[NiicKje]

Hello MANY thanks for the code above, search whole internet for it.
But if i put it on my nxt brick, i dont see it at Software Files..
can u put a link for the downloadable code? so i can just put That on my brick?
because i tried to make a new nxc file, but i cannot compile...
so please put a link with the program in it PLEASE

[dodgey99]

I can't upload files - I get an error and asked to contact the administrator of this forum. Send me a private message here with your email address and I'll send it to you asap.

Notes:

1) You really don't need to use the calibration any more , other than to set the sensor center position.
2) I've modified the code again, rather messily, so now it shows, when scanning the cube at startup, each colour by name, on the NXT screen - this happens fast, but it's very useful as a check - get someone to watch the screen and call out the colour names as the cube is initially scanned. They must obviously match. If they don't, align your colour sensor properly. Sometimes I find the sensor arm "droops" a bit and scans the very edge of cube faces, it still works though.
3) remember, you need to replace orange with black on your cube. I used sticky mailing labels, coloured them black with a marker pen, and then cut and stick them on.
4) The calibration program has pauses after each face scan, just press the orange NXT button to advance to the next face.
4b) I've done 6 cube solves now with a 100% success rate.

5) Finally - you SHOULD be able to compile. The reason it does not is you need to go into BrixxCC options , into the compiler tab, and then into the NXC/NBC tab and make sure NXT 2.0 compatibility is ticked. I just copy and pasted the above code into BrixxCC and it compiled 1st time.

Like I said, PM me and I'll email you the executables.

ADMIN! - Can I please have write access to upload files :-)

hmmm - now thinking about it I'm going to strip down calibrate.rxe to just calibrate the arm, and try and squeeze in 6 sound files, calling out the scanned colours :-)

[NiicKje]

I can't upload files - I get an error and asked to contact the administrator of this forum. Send me a private message here with your email address and I'll send it to you asap.

Notes:

1) You really don't need to use the calibration any more , other than to set the sensor center position.
2) I've modified the code again, rather messily, so now it shows, when scanning the cube at startup, each colour by name, on the NXT screen - this happens fast, but it's very useful as a check - get someone to watch the screen and call out the colour names as the cube is initially scanned. They must obviously match. If they don't, align your colour sensor properly. Sometimes I find the sensor arm "droops" a bit and scans the very edge of cube faces, it still works though.
3) remember, you need to replace orange with black on your cube. I used sticky mailing labels, coloured them black with a marker pen, and then cut and stick them on.
4) The calibration program has pauses after each face scan, just press the orange NXT button to advance to the next face.
4b) I've done 6 cube solves now with a 100% success rate.

5) Finally - you SHOULD be able to compile. The reason it does not is you need to go into BrixxCC options , into the compiler tab, and then into the NXC/NBC tab and make sure NXT 2.0 compatibility is ticked. I just copy and pasted the above code into BrixxCC and it compiled 1st time.

Like I said, PM me and I'll email you the executables.

ADMIN! - Can I please have write access to upload files :-)

i sended you a PM

[dodgey99]

All sorted (you need the latest firmware for this app to work).

Just for fun, when I can upload files I'll put up the zip for the app, and also a new one where it speaks the colours to you. I had to sacrifice gameover.rso and thankyou.rso to fit all the sounds in but it's kinda funky to hear it call out the colours, not to mention easier than reading the screen

[ronmcrae]

3) remember, you need to replace orange with black on your cube.

Have you looked at the raw RGB values for orange versus other colors? Maybe an additional user-implemented NXC function to do color determination from the RGB value would mean that a non-modified cube could be used.

[dodgey99]

Have you looked at the raw RGB values for orange versus other colors? Maybe an additional user-implemented NXC function to do color determination from the RGB value would mean that a non-modified cube could be used.

In theory it should be very simple - but in practice I was getting lots of inconsistencies, and as I'd already made the orange square black, it was super easy just to use colorval. The original code does exactly what you say - determines the "colour" by comparing rgb values - I just could not get it to work with the new NXT 2 sensor.

The problem is I don't understand the code used to determine different colours using the rgb values - they are multiplied in two instances, once by 9 and I really don't know why. I figure the HT colour sensor puts out quite different value ranges. I did a fair bit of dabbling and kept getting mis-reads. It got close, once it solved a cube but with 8 or 9 pieces out of place. Once I went for the colorval method I got 100% correct reads.

At the end of the day I chose a method that works consistently with the orange to black modification

[Yourani]

This worked perfectely for me.
I have NXT 1.0 block and the color sensor from the 2.0 kit
I updated the firmware of my NXT to 1.28 (download:http://www.legoengineering.com/)
and everything worked fine with the program i got from dodgey.

Yourani

[dodgey99]

Excellent news! I can attach files now so here is the speech version:

This worked perfectely for me.
I have NXT 1.0 block and the color sensor from the 2.0 kit
I updated the firmware of my NXT to 1.28 (download:http://www.legoengineering.com/)
and everything worked fine with the program i got from dodgey.

Yourani

Attached File(s)

 Tilted_Twister_Speech_Final.zip ( 25.14k ) Number of downloads: 132

 

[gallo]

Hello, I want to know some information about the resolution of the Rubik's cube with the NXT.
The robot recognizes correctly all the colors of all the faces, but when it enters the method SolveTopAndBottomEdges enter into loops and no longer able to leave.
Let me know if you know why, or if you can give me advice.


If you already solved the cube on the robot with the white face turned towards you, so the white one above the LCD, you can tell me what is face UPPERFACE, DOWNFACE and other .. I do not know if it gives me the right faces .. Thanks
Bye

[NXC_Newbie]

Hello everyone, I wanted to build the Tilted Twister and followed the link on Han's page but did not work. Please check if this link is correct: http://www.tiltedtwister.com/tiltedtwister...structions.html Many thanks!

[mlwatts]

Total Noob,
can somebody please post a rxe file for this as I have no idea what i'm supposed to do with the nxc files.
I can download them but i am unable to run the on the nxt.
help i'm such an idiot

[gallo]

hello, I would ask if anyone knew why my NXT that solves Rubik's cube at the end of the resolution can not accommodate the central square, but everything else is perfect, please help me .. thanks .. hello hello

color sensor 2.0
Fimware 1.28
Titled Twister version 4

[dhedgec]

Can someone post an rxe for tilted twister program with the color sensor 2.0 that does not require changing the stickers on the rubiks cube?
New to NXT but want to try the rubiks cube solver because it looked super cool!

[dodgey99]

Can someone post an rxe for tilted twister program with the color sensor 2.0 that does not require changing the stickers on the rubiks cube?
New to NXT but want to try the rubiks cube solver because it looked super cool!

There isn't a version out there yet for what you need. I tried amending it so it would use the RGB values to determine a colour value for each face, much the same as it does using the hitechnic colour sensor, but I just could not get it to work.

You are going to have to try this yourself.

Hint - if you use my amended code, look for where I take the readings for SensorColorEX - I take the "Colorval" reading which is 1-6 depending on the colour. You need to use the "raw" field instead to manipulate the RGB values. Best of luck with it :-)

p.s. All you need to do is put postage stickers on the orange face and colour them with a black marker pen and you are good to go on the existing code. Much much easier!

[marshen]

Problem

Its scans the first face then says woops.
How do i fix this im using V4 of both calibrate and Tilted twister?

[dodgey99]

That's because the ultrasonic sensor is not seeing the cube. After each face scan (and twist) it checks for the cube. Might want to check it is in the right port and in the right place.

If you look at the source code you can change the sensitivity of the sensor but you should not need to change it.

p.s. It also does exactly as you describe if your batteries are low.

Problem

Its scans the first face then says woops.
How do i fix this im using V4 of both calibrate and Tilted twister?

[2010]

Total Noob,
can somebody please post a rxe file for this as I have no idea what i'm supposed to do with the nxc files.
I can download them but i am unable to run the on the nxt.
help i'm such an idiot

Same problem here, sorry but how can I run the .nxc- files (calibration and the programm itself)-how can I import it to NXT 2.0??

Many thanks!

Tom

[dodgey99]

Same problem here, sorry but how can I run the .nxc- files (calibration and the programm itself)-how can I import it to NXT 2.0??

Many thanks!

Tom

You need to compile them in Brixcc - search this forum - it's all here