Tilted Twister with HiTechnic Color Sensor

Discussion specific to NXT-G, NXC, NBC, RobotC, Lejos, and more.
Post Reply
samuelwhistler
Posts: 4
Joined: 25 Nov 2010, 21:31

Tilted Twister with HiTechnic Color Sensor

Post by samuelwhistler »

Hey,

i have build the tilted twister (http://tiltedtwister.com) from hans andersson, but i need help with the program(change the original color sensor with the hitechnic color sensor). Can anyone show me what i have to change in the nxc-prog?

NXC-Prog: http://tiltedtwister.com/download/tt2/tiltedtwister.nxc

best regards
Top
mattallen37
Posts: 1818
Joined: 02 Oct 2010, 02:19
Location: Michigan USA
Contact:
Contact mattallen37

Re: Tilted Twister with HiTechnic Color Sensor

Post by mattallen37 »

Originally it was for the HiTechnic sensor (I think v1), so the program should already exist.
Matt
http://mattallen37.wordpress.com/

I'm all for gun control... that's why I use both hands when shooting ;)
Top
samuelwhistler
Posts: 4
Joined: 25 Nov 2010, 21:31

Re: Tilted Twister with HiTechnic Color Sensor

Post by samuelwhistler »

that's right,
but i have problems to implement the part for the colorSensor (from oldversion) in the new 2.0 version of tiltedtwister .
maybe anyone can try to overwrite the part with the sensor in the new version(my knowledge about NXC is not the best).
Last edited by samuelwhistler on 27 Nov 2010, 22:09, edited 1 time in total.
Top
mattallen37
Posts: 1818
Joined: 02 Oct 2010, 02:19
Location: Michigan USA
Contact:
Contact mattallen37

Re: Tilted Twister with HiTechnic Color Sensor

Post by mattallen37 »

What sensor do you have? Now you make it sound like you have the lego 2.0 color sensor, not the HT sensor.
Matt
http://mattallen37.wordpress.com/

I'm all for gun control... that's why I use both hands when shooting ;)
Top
samuelwhistler
Posts: 4
Joined: 25 Nov 2010, 21:31

Re: Tilted Twister with HiTechnic Color Sensor

Post by samuelwhistler »

no no , i have the Hitechnic Color Sensor.

I may have found a solution(okay its no solution ;-) , but a step forward). i modified the code for the procedure ReadColor:


void ReadColor(int face, int pos)
{
int colorval;
unsigned char cidx[];
unsigned int rawr[];
unsigned int rawg[];
unsigned int rawb[];
unsigned int normr[];
unsigned int normg[];
unsigned int normb[];

while(!ReadSensorHTRawColor(IN_1, rawr, rawg, rawb));
while(!ReadSensorHTNormalizedColor(IN_1, cidx, normr, normg, normb));
int tile=Tile(face,pos);
cubeColor[tile].colorval=colorval;
cubeColor[tile].normRed=normr[0];
cubeColor[tile].normGreen=normg[1];
cubeColor[tile].normBlue=normb[2];
cubeColor[tile].rawRed=rawr[0];
cubeColor[tile].rawGreen=rawg[1];
cubeColor[tile].rawBlue=rawb[2];
}


the compiler no longer stops now, but when i test, the NXT shows FileError on LCD (after he rotates the scanner over the cube and before he would start to scan).
Top
HaWe
Posts: 2500
Joined: 04 Nov 2014, 19:00

Re: Tilted Twister with HiTechnic Color Sensor

Post by HaWe »

hi,
maybe it makes sense to have Hans Andersson's code here for a quicker access to it:

Code: Select all

/*
Tilted Twister, http://tiltedtwister.com
Hans Andersson 2010-08-15

Wiring:
1 Color sensor
2 Ultrasonic sensor
A Center motor
B Tilter arm motor
C Color sensor motor
*/

#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 SENSORPOS_HOME      50
#define SENSORPOS_CENTER    150
#define SENSORPOS_CORNERS   177
#define SENSORPOS_EDGES     172
#define SENSORPOS_LASTEDGE  130

#define OFFSET_EDGE       35
#define OFFSET_LASTEDGE   200

#define TILTREST  800	// Time (ms) to rest before return tilterarm
#define SCANSPEED 40

#define MAXMOVES  300

#define BT_CONN 2
#define INBOX 5
#define OUTBOX 1

#define REMOTE_MAXTIME 10000

#define REMOTESOLVE_OK      0
#define REMOTESOLVE_ERROR   1
#define REMOTESOLVE_ABSENT  2

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};

struct colorType
{
  int colorval;
  unsigned int rawRed;
  unsigned int rawGreen;
  unsigned int rawBlue;
  unsigned int normRed;
  unsigned int normGreen;
  unsigned int normBlue;
};

colorType cubeColor[6*9];
char cube[6*9];
char tmpCube[6*9];
char moves[MAXMOVES];
int  movesCount=0;
char solution[MAXMOVES];
int  solutionCount;
int  solutionTwists;
int  twists;
char staticfaces[]={'L','F','R','B','U','D'};
char faces[]={'L','F','R','B','U','D'};
bool cubeGrabbed;
int  currentAngle; //Current position of turntable
int  tiltPower=85; //Initial tiltpower. Continuously adjusted depending on actual speed
int  grabPower;

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

rgb refColor[6];
rgb sensorColor[6*9];
unsigned long costMatrix[12*12];
int twistMatrix[12*12];
byte color[6*9];

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

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


inline unsigned long 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];
}


inline int Tile(int face, int pos)
{
  return 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];
}


void Tilt()
{
  long tachoPrevious,tachoNow;
  unsigned long tick,time;
  Coast(OUT_A);
  Wait(200);
  tick=CurrentTick();
  OnFwd(OUT_B,tiltPower);
  while(MotorRotationCount(OUT_B)<65);
  time=CurrentTick()-tick;
  Off(OUT_B);
  if(time > 215)
  {
    tiltPower += 1 + (time-215)/10;
    if(tiltPower>100)
      tiltPower=100;
  }
  else
    tiltPower--;
  grabPower=tiltPower/11;
  Wait(TILTREST);
  OnFwd(OUT_B,-100);
  while(MotorRotationCount(OUT_B)>40);
  OnFwd(OUT_B,-20);
  while(MotorTachoCount(OUT_B)>20);
  tachoNow=MotorTachoCount(OUT_B);
  do{
    tachoPrevious=tachoNow;
    Wait(10);
    tachoNow=MotorTachoCount(OUT_B);
  }while(tachoNow!=tachoPrevious);
  Coast(OUT_B);
  char uf=faces[UPPERFACE];
  faces[UPPERFACE]=faces[LEFTFACE];
  faces[LEFTFACE]=faces[DOWNFACE];
  faces[DOWNFACE]=faces[RIGHTFACE];
  faces[RIGHTFACE]=uf;
}


void GrabCube()
{
  Coast(OUT_A);
  OnFwd(OUT_B,100);
  while(MotorRotationCount(OUT_B)<20);
  OnFwd(OUT_B,20);
  while(MotorRotationCount(OUT_B)<30);
  OnFwd(OUT_B,grabPower);
  Wait(300);
  cubeGrabbed=true;
}


void ReleaseCube()
{
  int tachoNow,tachoPrevious;
  Off(OUT_A);
  Off(OUT_B);
  OnFwd(OUT_B,-100);
  while(MotorRotationCount(OUT_B)>20);
  OnFwd(OUT_B,-20);
  while(MotorTachoCount(OUT_B)>20);
  tachoNow=MotorTachoCount(OUT_B);
  do
  {
    tachoPrevious=tachoNow;
    Wait(10);
    tachoNow=MotorTachoCount(OUT_B);
  }while(tachoNow!=tachoPrevious);
  Coast(OUT_B);
  cubeGrabbed=false;
}


void TurnTo(int newAngle)
{
  int direction;
  int angle1,angle2;
  long pwr;
  int speed;
  int distance;
  if(newAngle!=currentAngle)
  {
    if(newAngle==0) //home
    {
      OnFwd(OUT_A,100);
      do
      {
        angle1=MotorRotationCount(OUT_A);
        Wait(20);
        angle2=MotorRotationCount(OUT_A);
        speed=abs(angle2-angle1);
        distance=abs(newAngle-angle2);
        if(speed>11)
        {
          pwr=20+distance;
          if(pwr<100)
            OnFwd(OUT_A,pwr);
        }
      }while(distance>30 || angle2!=angle1);
      Off(OUT_A);
      Wait(300);
    }
    else
    {
      int angle=newAngle - MotorRotationCount(OUT_A);
      ResetTachoCount(OUT_A);
      Wait(50);
      RotateMotor(OUT_A,60,angle);
      Wait(300);
    }
    int angle=MotorRotationCount(OUT_A);
    if(!cubeGrabbed)
    {
      int num=(4+(currentAngle-newAngle)/90)%4;
      for(int i=0;i<num;i++)
      {
        char lf=faces[LEFTFACE];
        faces[LEFTFACE]=faces[FRONTFACE];
        faces[FRONTFACE]=faces[RIGHTFACE];
        faces[RIGHTFACE]=faces[BACKFACE];
        faces[BACKFACE]=lf;
      }
    }
    currentAngle=newAngle;
  }
}


inline void TurnQuarter(int num)
{
  TurnTo((currentAngle-num*90)%360);
}


void Twist(int num)
{
  int angle=num * -90;
  if(angle==currentAngle)
    Wait(TILTREST);
  else
    TurnTo(angle);
  GrabCube();
  TurnTo(0);
  ReleaseCube();
}


//*****************************************************************************
// Initialisation

void InitTurntable()
{
  int tachoNow,tachoPrevious;
  OnFwd(OUT_A,15);
  do
  {
    tachoPrevious=tachoNow;
    Wait(100);
    tachoNow=MotorTachoCount(OUT_A);
  }while(tachoNow!=tachoPrevious);
  Coast(OUT_A);
  Wait(500);
  ResetAllTachoCounts(OUT_A);
  Wait(500);
  currentAngle=0;
}


void InitTilter()
{
  int tachoNow,tachoPrevious;
  OnFwd(OUT_B,-20);
  tachoNow=MotorTachoCount(OUT_B);
  do
  {
    tachoPrevious=tachoNow;
    Wait(100);
    tachoNow=MotorTachoCount(OUT_B);
  }while(tachoNow!=tachoPrevious);
  Coast(OUT_B);
  Wait(500);
  ResetAllTachoCounts(OUT_B);
}


void InitScanner()
{
  int tachoNow,tachoPrevious;
  OnFwd(OUT_C,-15);
  tachoNow=MotorTachoCount(OUT_C);
  do
  {
    tachoPrevious=tachoNow;
    Wait(100);
    tachoNow=MotorTachoCount(OUT_C);
  }while(tachoNow!=tachoPrevious);
  Off(OUT_C);
  Wait(500);
  ResetAllTachoCounts(OUT_C);
  Wait(500);
  RotateMotor(OUT_C,10,SENSORPOS_HOME);
}


void Initialize()
{
  InitTilter();
  InitScanner();
  InitTurntable();
  SetSensorLowspeed(IN_2);
}


//*****************************************************************************
// Scan cube

inline void DrawFacelet(int pos)
{
  RectOut(59-(pos/3)*17,42-(pos%3)*17,15,15);
}


void ReadColor(int face, int pos)
{
  int colorval;
  unsigned int raw[];
  unsigned int norm[];
  int scaled[];
  while(!ReadSensorColorEx(IN_1,colorval,raw,norm,scaled));
  int tile=Tile(face,pos);
  cubeColor[tile].colorval=colorval;
  cubeColor[tile].normRed=norm[0];
  cubeColor[tile].normGreen=norm[1];
  cubeColor[tile].normBlue=norm[2];
  cubeColor[tile].rawRed=raw[0];
  cubeColor[tile].rawGreen=raw[1];
  cubeColor[tile].rawBlue=raw[2];
}


void ScanFace(int face,int c1,int c2,int c3,int c4,int c5,int c6,int c7,int c8,int endAngle)
{
  TurnTo(0);
  ResetTachoCount(OUT_A);
  ClearScreen();
  RotateMotorEx(OUT_C,20,SENSORPOS_CENTER-SENSORPOS_HOME,0,false,false);
  ReadColor(face,CENTER);
  DrawFacelet(CENTER);
  RotateMotor(OUT_C,20,SENSORPOS_CORNERS-SENSORPOS_CENTER);
  ReadColor(face,c1);
  DrawFacelet(UPPERRIGHT);
  RotateMotorEx(OUT_A,SCANSPEED,-90,0,false,false);
  ReadColor(face,c2);
  DrawFacelet(DOWNRIGHT);
  RotateMotorEx(OUT_A,SCANSPEED,-90,0,false,false);
  ReadColor(face,c3);
  DrawFacelet(DOWNLEFT);
  RotateMotor(OUT_A,SCANSPEED,-90);
  ReadColor(face,c4);
  DrawFacelet(UPPERLEFT);
  RotateMotor(OUT_C,10,SENSORPOS_EDGES-SENSORPOS_CORNERS);
  RotateMotorEx(OUT_A,SCANSPEED,OFFSET_EDGE,0,false,false);
  ReadColor(face,c5);
  DrawFacelet(MIDLEFT);
  RotateMotorEx(OUT_A,SCANSPEED,90,0,false,false);
  ReadColor(face,c6);
  DrawFacelet(UPPERMID);
  RotateMotorEx(OUT_A,SCANSPEED,90,0,false,false);
  ReadColor(face,c7);
  DrawFacelet(MIDRIGHT);
  RotateMotor(OUT_A,SCANSPEED,OFFSET_LASTEDGE - 270 - OFFSET_EDGE);
  RotateMotorEx(OUT_C,20,SENSORPOS_LASTEDGE-SENSORPOS_EDGES,0,false,false);
  ReadColor(face,c8);
  DrawFacelet(DOWNMID);
  RotateMotor(OUT_C,20,SENSORPOS_HOME-SENSORPOS_LASTEDGE);
  currentAngle=-1;
  TurnTo(endAngle);
}


void ScanCube()
{
  SetSensorColorFull(IN_1);
  ScanFace(BACKFACE,UPPERRIGHT,UPPERLEFT,DOWNLEFT,DOWNRIGHT,DOWNMID,MIDLEFT,UPPERMID,MIDRIGHT,0);
  Tilt();
  ScanFace(LEFTFACE,UPPERRIGHT,UPPERLEFT,DOWNLEFT,DOWNRIGHT,DOWNMID,MIDLEFT,UPPERMID,MIDRIGHT,0);
  Tilt();
  ScanFace(FRONTFACE,UPPERRIGHT,UPPERLEFT,DOWNLEFT,DOWNRIGHT,DOWNMID,MIDLEFT,UPPERMID,MIDRIGHT,-270);
  Tilt();
  ScanFace(DOWNFACE,UPPERRIGHT,UPPERLEFT,DOWNLEFT,DOWNRIGHT,DOWNMID,MIDLEFT,UPPERMID,MIDRIGHT,0);
  Tilt();
  ScanFace(RIGHTFACE,UPPERLEFT,DOWNLEFT,DOWNRIGHT,UPPERRIGHT,MIDRIGHT,DOWNMID,MIDLEFT,UPPERMID,0);
  Tilt();
  ScanFace(UPPERFACE,DOWNLEFT,DOWNRIGHT,UPPERRIGHT,UPPERLEFT,UPPERMID,MIDRIGHT,DOWNMID,MIDLEFT,0);
  SetSensorType(IN_1,SENSOR_TYPE_NONE);
}


//*****************************************************************************
// Color resolving

void Sorry()
{
  TextOut(5,LCD_LINE2,"SORRY",true);
  TextOut(5,LCD_LINE4,"Can't resolve" );
  TextOut(5,LCD_LINE5,"colors" );
  PlayFile("Sorry.rso");
}


int GetRefCornerColor(rgb refColor1,rgb refColor2)
{
  int pos3;
  unsigned long minCost=$FFFFFFFF;
  for(int corner=1;corner<8;corner++)
  {
    int minCostTwist;
    for(int twist=0;twist<3;twist++)
    {
      int pos1 = CornerGet(corner,twist);
      int pos2 = CornerGet(corner,(1+twist)%3);
      rgb color1 = sensorColor[pos1];
      rgb color2 = sensorColor[pos2];
      unsigned long cost = ((refColor1.red - color1.red) * (refColor1.red - color1.red) + (refColor1.green - color1.green) * (refColor1.green - color1.green) + (refColor1.blue - color1.blue) * (refColor1.blue - color1.blue))/3;
      cost += ((refColor2.red - color2.red) * (refColor2.red - color2.red) + (refColor2.green - color2.green) * (refColor2.green - color2.green) + (refColor2.blue - color2.blue) * (refColor2.blue - color2.blue))/3;
      if(cost<minCost)
      {
        minCost=cost;
        minCostTwist=twist;
        pos3=CornerGet(corner,(2+twist)%3);
      }
    }
  }
  return pos3;
}


void InitColors()
{
  for(int pos=0;pos<6*9;pos++)
  {
    sensorColor[pos].red=cubeColor[pos].rawRed;
    sensorColor[pos].green=cubeColor[pos].rawGreen;
    sensorColor[pos].blue=cubeColor[pos].rawBlue;
  }
  refColor[UPPERFACE]=sensorColor[UPPERFACE_UPPERLEFT];
  refColor[LEFTFACE]=sensorColor[LEFTFACE_UPPERLEFT];
  refColor[BACKFACE]=sensorColor[BACKFACE_UPPERRIGHT];
  refColor[FRONTFACE]=sensorColor[GetRefCornerColor(refColor[LEFTFACE],refColor[UPPERFACE])];
  refColor[RIGHTFACE]=sensorColor[GetRefCornerColor(refColor[UPPERFACE],refColor[BACKFACE])];
  refColor[DOWNFACE]=sensorColor[GetRefCornerColor(refColor[BACKFACE],refColor[LEFTFACE])];
}


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


void ResolveCenterColors()
{
  TextOut(2,LCD_LINE4,"Center colors...");
  for(int center=0;center<6;center++)
  {
    unsigned long minCost=$FFFFFFFF;
    int bestCubie;
    for(int cubie=0;cubie<6;cubie++)
    {
      rgb centerColor,cubieColor;
      centerColor = refColor[center];
      cubieColor=sensorColor[Tile(cubie,CENTER)];
      unsigned long 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);
}


int BestFitCubie(int &cubie, int &twist, int dim)
{
  int bestX,bestY;
  unsigned long bestDiff=0;
  for(int x = 0; x < dim;x++)
  {
    unsigned long minCost=$FFFFFFFF;
    unsigned long min2Cost=$FFFFFFFF;
    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,$FFFFFFFF);
  for(int y=0;y<dim;y++)
    CostMatrixSet(bestX,y,$FFFFFFFF);

  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 long minCost=$FFFFFFFF;
      int minCostTwist;
      for(int twist=0;twist<3;twist++)
      {
        unsigned long cost=0;
        for(int s=0;s<3;s++)
        {
          int face = CornerGet(corner,s) / 9;
          int pos = CornerGet(cubie,(s+twist)%3);
          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;
        }
        if(cost<minCost)
        {
          minCost=cost;
          minCostTwist=twist;
        }
      }
      CostMatrixSet(corner,cubie,minCost);
      TwistMatrixSet(corner,cubie,minCostTwist);
    }
    for(int face=0;face<6;face++) //calibrate refColor
    {
      refColor[face].red=0;
      refColor[face].green=0;
      refColor[face].blue=0;
    }
    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;
        //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;
      }
    }
    //calibrate refColor
    for(int face = 0 ; face < 6 ;face++)
    {
      rgb tmp=refColor[face];
      tmp.red/=4;
      tmp.green/=4;
      tmp.blue/=4;
      refColor[face]=tmp;
    }
}


void ResolveEdgeColors()
{
  TextOut(2,LCD_LINE6,"Edge colors...");
  for(int edge=0;edge<12;edge++)
    for(int cubie=0;cubie<12;cubie++)
    {
      unsigned long minCost=$FFFFFFFF;
      int minCostTwist;
      for(int twist=0;twist<2;twist++)
      {
        unsigned long cost=0;
        for(int s=0;s<2;s++)
        {
          int face = EdgeGet(edge,s)/ 9;
          int pos = EdgeGet(cubie,(s+twist)%2);
          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;
        }
        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;
    }
}


string ColorToStr(int face, int pos)
{
  byte c=color[Tile(face,pos)];
  return SubStr("LFRBUD",c,1);
}


void DumpCube()
{
  int handle;
  DeleteFile("rubikscube.txt");
  CreateFile("rubikscube.txt", 4000, handle);
  int count;
  string msg,c;
  for(int row=0;row<3;row++)
  {
    msg="    ";
    for(int col=0;col<3;col++)
    {
      c=ColorToStr(UPPERFACE,row*3+col);
      msg=StrCat(msg,c);
    }
    WriteLnString(handle,msg,count);
  }
  WriteLnString(handle,"",count);
  for(int row=0;row<3;row++)
  {
    msg="";
    for(int face=LEFTFACE;face<=BACKFACE;face++)
    {
      for(int col=0;col<3;col++)
      {
        c=ColorToStr(face,row*3+col);
        msg=StrCat(msg,c);
      }
      msg=StrCat(msg," ");
    }
    WriteLnString(handle,msg,count);
  }
  WriteLnString(handle,"",count);
  for(int row=0;row<3;row++)
  {
    msg="    ";
    for(int col=0;col<3;col++)
    {
      c=ColorToStr(DOWNFACE,row*3+col);
      msg=StrCat(msg,c);
    }
    WriteLnString(handle,msg,count);
  }
  WriteLnString(handle,"",count);

  WriteLnString(handle,"colorval Raw-RGB [Norm-RGB] = Resolved face",count);
  WriteLnString(handle,"",count);
  for(int face=0;face<6;face++)
  {
    msg=SubStr("LFRBUD",face,1); WriteString(handle,msg,count); WriteString(handle,": ",count);
    for(int pos=0;pos<9;pos++)
    {
      colorType col=cubeColor[Tile(face,pos)];
      msg=NumToStr(col.colorval); WriteString(handle,msg,count); WriteString(handle," ",count);
      msg=NumToStr(col.rawRed); WriteString(handle,msg,count); WriteString(handle,".",count);
      msg=NumToStr(col.rawGreen); WriteString(handle,msg,count); WriteString(handle,".",count);
      msg=NumToStr(col.rawBlue); WriteString(handle,msg,count); WriteString(handle," [",count);
      msg=NumToStr(col.normRed); WriteString(handle,msg,count); WriteString(handle,".",count);
      msg=NumToStr(col.normGreen); WriteString(handle,msg,count); WriteString(handle,".",count);
      msg=NumToStr(col.normBlue); WriteString(handle,msg,count); WriteString(handle,"]=",count);
      msg=ColorToStr(face,pos); WriteString(handle,msg,count); WriteString(handle,", ",count);
    }
    WriteLnString(handle,"",count);
  }
  WriteLnString(handle,"",count);
  WriteLnString(handle,"Ref RGB",count);
  WriteLnString(handle,"",count);
  for(int face=0;face<6;face++)
  {
    msg=SubStr("LFRBUD",face,1); WriteString(handle,msg,count); WriteString(handle,": ",count);
    rgb col=refColor[face];
    msg=NumToStr(col.red); WriteString(handle,msg,count); WriteString(handle,".",count);
    msg=NumToStr(col.green); WriteString(handle,msg,count); WriteString(handle,".",count);
    msg=NumToStr(col.blue); WriteLnString(handle,msg,count);
  }
  CloseFile(handle);
}


void ResolveColors()
{
  TextOut(2,LCD_LINE2,"RESOLVING COLORS",true);
  InitColors();
  ResolveCenterColors();
  ResolveCornerColors();
  ResolveEdgeColors();
  DumpCube();
}

//*****************************************************************************
// Solve functions

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 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)
{
  if(movesCount+turns>=MAXMOVES)
  {
    Sorry();
    Wait(10000);
    Stop(true);
  }
  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));
    }
    moves[movesCount++]=cube[DOWNFACE_CENTER];
  }
  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;
}


//*****************************************************************************

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


int RemoteSolve()
{
  if(BluetoothStatus(0)==NO_ERR)
  {
    TextOut(10,LCD_LINE1,"Connecting...",true);
    LoadCube();
    string cubeString=ByteArrayToStr(cube);
    SendResponseString(OUTBOX,cubeString);
    string result;
    unsigned long tick;
    tick=CurrentTick();
    while(ReceiveRemoteString(INBOX, true, result) == STAT_MSG_EMPTY_MAILBOX && CurrentTick() < tick + REMOTE_MAXTIME);
    if(StrLen(result)==0)
      return REMOTESOLVE_ABSENT;
    else if(result=="ERROR")
    {
      Sorry();
      return REMOTESOLVE_ERROR;
    }
    else
    {
      for(int i=0;i<StrLen(result);i++)
        moves[i]=result[i];
      movesCount=StrLen(result);
      Optimize();
      SaveSolution();
      return REMOTESOLVE_OK;
    }
  }
  else
    return REMOTESOLVE_ABSENT;
}


bool SolveCube()
{
  int remoteSolve=RemoteSolve();
  if(remoteSolve==REMOTESOLVE_OK)
    return true;
  else if (remoteSolve==REMOTESOLVE_ERROR)
    return false;
  else
  {
    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();
    return true;
  }
}

//*****************************************************************************
// Execute Moves

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': TurnQuarter(2); Tilt(); break;
    case 'F': TurnQuarter(3);Tilt(); break;
    case 'R': Tilt(); break;
    case 'B': TurnQuarter(1); 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);
  }
  Wait(1000);
  TextOut(20,LCD_LINE4,"GAME OVER",true);
  PlayFile("Game Over.rso");
  Wait(2000);
  Coast(OUT_A);
}


//*****************************************************************************

void WaitForCube()
{
  TextOut(7,LCD_LINE4,"GIVE ME A CUBE",true);
  Wait(2000);
  unsigned long startTick=CurrentTick();
  do
  {
    if(SensorUS(IN_2) > 10)
      startTick=CurrentTick();
  }while(CurrentTick() - startTick < 1000 && !ButtonPressed(BTNCENTER,false));
  TextOut(20,LCD_LINE5,"THANK YOU !",true);
  PlayFile("Thank You.rso");
  Wait(2000);
}


void WaitForCubeRemove()  //Wait until cube absent for three seconds or button pressed
{
  unsigned long startTick=CurrentTick();
  do
  {
    if(SensorUS(IN_2) < 10)
      startTick=CurrentTick();
  }while(CurrentTick() - startTick < 3000 && !ButtonPressed(BTNCENTER,false));
}


task main()
{
  while(true)
  {
    Initialize();
    WaitForCube();
    ScanCube();
    ResolveColors();
    if(SolveCube())
      DoMoves();
    WaitForCubeRemove();
  }
}
Top
afanofosc
Site Admin
Posts: 1256
Joined: 26 Sep 2010, 19:36
Location: Nashville, TN
Contact:
Contact afanofosc

Re: Tilted Twister with HiTechnic Color Sensor

Post by afanofosc »

The problem is that the two HT Color sensor functions you are using do not take arrays. The NBC/NXC compiler is a little lax (understatement) in checking the types that you are passing in. Here's code that might work:

Code: Select all

void ReadColor(int face, int pos)
{
byte cidx;
unsigned int rawr;
unsigned int rawg;
unsigned int rawb;
byte normr;
byte normg;
byte normb;

while(!ReadSensorHTRawColor(IN_1, rawr, rawg, rawb));
while(!ReadSensorHTNormalizedColor(IN_1, cidx, normr, normg, normb));
int tile=Tile(face,pos);
cubeColor[tile].colorval=cidx;
cubeColor[tile].normRed=normr;
cubeColor[tile].normGreen=normg;
cubeColor[tile].normBlue=normb;
cubeColor[tile].rawRed=rawr;
cubeColor[tile].rawGreen=rawg;
cubeColor[tile].rawBlue=rawb;
}
While the compiler isn't helping you at all with respect to the correct types here, the NXC documentation does tell you what the right types are and if you have the HTML help properly integrated with the BricxCC IDE then you can get parameter insight by pressing Ctrl+Space inside the function call parenthesis and context-sensitive help on the function by pressing F1 while the edit cursor is on the function name.

John Hansen
Multi-platform LEGO MINDSTORMS programming
http://bricxcc.sourceforge.net/
Top
samuelwhistler
Posts: 4
Joined: 25 Nov 2010, 21:31

Re: Tilted Twister with HiTechnic Color Sensor

Post by samuelwhistler »

it works, but the calibration is difficult
thank you
Top
Post Reply

Return to “Mindstorms Software”

Who is online

Users browsing this forum: No registered users and 5 guests