|
|
发表于 2017-3-19 15:37:45
|
显示全部楼层
/*
LEGO Mindstorms Sudoku Solver
Hans Andersson 2009
http://tiltedtwister.com
Version 1.0 (2009-09-13)
Inputs:
2 Light sensor
Outputs:
A Back motor
B Front motor
C Pen elevator motor
*/
///////////////////////////////////////////////////////////////////////////////
#define SIZE_Y 25 //Number of lines to scan for each digit. To speed up scanning, this value could be set to 20 or lower (but with higher risk of failing)
#define SIZE_X 32 //width of scanned digit
#define STEP_X 1 //motor steps between reading lightsensor.
#define STEP_Y 20 //motor steps between scanned lines
#define SPEED_X 70 //motor power for arm when scanning
#define SPEED_Y 70 //motor power for wheels when scanning
#define PENSPEED_X 20 //motor power for arm when drawing
#define PENSPEED_Y 30 // motor power for wheels when drawing
#define PUZZLE_WIDTH 1300
#define CALIBRATE_WIDTH 400
#define CELLHEIGHT 560
#define PROBE_LIMIT 3
#define BEFORE 20
#define NORTH 1
#define NORTHEAST 2
#define EAST 3
#define SOUTHEAST 4
#define SOUTH 5
#define SOUTHWEST 6
#define WEST 7
#define NORTHWEST 8
#define MAX_LEVEL 65
#define ONE 1
#define TWO 2
#define THREE 4
#define FOUR 8
#define FIVE 16
#define SIX 32
#define SEVEN 64
#define EIGHT 128
#define NINE 256
#define Pixel(int pos) RectOut(18+(pos%SIZE_X)*2,(pos/SIZE_X)*2,1,1);
int leftToRightOffset,rightToLeftOffset;
byte scanbuffer[PUZZLE_WIDTH];
int sensorCellX[] = {-590,-425,-280,-135,0,135,280,425,590};
int sensorCellY[] = {-590,-310,-150,-50,0,-50,-150,-310,-590};
int penCellX[] = {-390,-290,-190,-95,0,95,190,290,390};
int penCellY[] = {-390,-220,-80,-35,0,-35,-80,-220,-390};
bool probe[9*9];
byte bmp[SIZE_X*SIZE_Y];
byte image[SIZE_X*SIZE_Y];
int sudoku[9*9];
int eliminatedDigitsInRow[9];
int eliminatedDigitsInCol[9];
int eliminatedDigitsInBox[9];
int eliminatedDigitsInCell[9*9];
int row[MAX_LEVEL];
int col[MAX_LEVEL];
int box[MAX_LEVEL];
int cell[MAX_LEVEL];
int digit[MAX_LEVEL];
int bitCount[512];
int GetCalibrateOffset(int direction)
{
int minVal=999;
int minPosStart=0;
int minPosEnd;
for(int i=0;i<CALIBRATE_WIDTH;i++)
{
byte light;
RotateMotorEx(OUT_B,SPEED_X,direction,0,false,false);
light=Sensor(IN_2);
scanbuffer[i]=light;
}
Off(OUT_B);
for(int i=0;i<CALIBRATE_WIDTH;i++)
if(scanbuffer[i] < minVal)
{
minVal=scanbuffer[i];
minPosStart=i;
minPosEnd=i;
}
else if(scanbuffer[i] == minVal)
minPosEnd=i;
return (minPosStart+minPosEnd)/2 - CALIBRATE_WIDTH/2;
}
void Calibrate()
{
RotateMotor(OUT_B,SPEED_X,-1*CALIBRATE_WIDTH/2);
leftToRightOffset=GetCalibrateOffset(1);
rightToLeftOffset=GetCalibrateOffset(-1);
RotateMotor(OUT_B,SPEED_X,CALIBRATE_WIDTH/2);
}
void PenDown()
{
RotateMotor(OUT_C,20,13);
}
void PenUp()
{
RotateMotor(OUT_C,20,-13);
}
void TestPen() //Test if pen is working (it's frustrating to notice it's not working after the scanning process)
{
RotateMotor(OUT_B,PENSPEED_X,-20);
PenDown();
RotateMotor(OUT_B,PENSPEED_X,40);
PenUp();
RotateMotor(OUT_B,PENSPEED_X,-20);
Wait(3000);
}
void Init()
{
byte maxLight=0;
byte light;
SetSleepTime(0); //Disable automatic shutdown
TestPen();
RotateMotor(OUT_A,100,5200);
SetSensorLight(IN_2);
Calibrate();
RotateMotorEx(OUT_A,30,-800,0,false,false);
do
{
RotateMotorEx(OUT_A,30,-10,0,false,false);
light=Sensor(IN_2);
if(light>maxLight)
maxLight=light;
}while(light>(maxLight-5))
Off(OUT_A);
Wait(1000);
ResetRotationCount(OUT_A);
Wait(1000);
}
void ProbeCell(int cell,int center)
{
int left=center-50;
int x = cell%9;
int y = 8-cell/9;
RectOut(18+x*7,y*7,7,7);
if(left<0)
left=0;
int right=left+100;
if (right>PUZZLE_WIDTH)
{
right=PUZZLE_WIDTH;
left=right-100;
}
int maxValue=scanbuffer[left];
int minValue=maxValue;
int i;
//Find decrease
for(i=left+1;i<right;i++)
{
if(scanbuffer[i]> maxValue)
{
maxValue=scanbuffer[i];
minValue=maxValue;
}
else if(scanbuffer[i]< minValue)
minValue=scanbuffer[i];
if(maxValue-minValue >= PROBE_LIMIT)
break;
}
//Find increase
maxValue=minValue;
for(;i<right;i++)
{
if(scanbuffer[i]< minValue)
{
minValue=scanbuffer[i];
maxValue=minValue;
}
else if(scanbuffer[i]> maxValue)
maxValue=scanbuffer[i];
if(maxValue-minValue >= PROBE_LIMIT)
{
probe[cell]=true;
RectOut(20+x*7,y*7+2,3,3);
RectOut(21+x*7,y*7+3,1,1);
return;
}
}
probe[cell]=false;
}
void FindNonEmptyCells()
{
int probeSequenceR[] = {0,-7,-6,-5,-4,-3,8};
int probeSequenceL[] = {-2,-8};
int probeColR[] = {0,2,3,4,5,6,8};
int probeColL[] = {7,1};
RotateMotor(OUT_B,SPEED_X,-1*PUZZLE_WIDTH/2);
RotateMotor(OUT_A,-50,CELLHEIGHT/7);
for(int i=9;i>=0;i--)
{
RotateMotor(OUT_A,-50,3*CELLHEIGHT/7);
for(int x=0;x<PUZZLE_WIDTH;x++)
{
byte light;
RotateMotorEx(OUT_B,SPEED_X,1,0,false,false);
light=Sensor(IN_2);
scanbuffer[x]=light;
}
Off(OUT_B);
for(int j=0;j<7;j++)
{
int pos=i*9+probeSequenceR[j];
if(pos < 81 && pos >= 0)
{
int x = PUZZLE_WIDTH/2+sensorCellX[probeColR[j]]+leftToRightOffset;
ProbeCell(pos,x);
}
}
RotateMotor(OUT_A,-50,4*CELLHEIGHT/7);
for(int x=PUZZLE_WIDTH-1 ;x>=0;x--)
{
byte light;
RotateMotorEx(OUT_B,SPEED_X,-1,0,false,false);
light=Sensor(IN_2);
scanbuffer[x]=light;
}
Off(OUT_B);
for(int j=0;j<2;j++)
{
int pos=i*9+probeSequenceL[j];
if(pos >= 0)
{
int x = PUZZLE_WIDTH/2 + sensorCellX[probeColL[j]]-rightToLeftOffset;
ProbeCell(pos,x);
}
}
}
int count=0;
for(int i=0;i<9*9;i++)
if(probe[i])
count++;
if(count<17)
{
PlayTone(400,3000);
TextOut(90,LCD_LINE2,"E");
TextOut(90,LCD_LINE3,"R");
TextOut(90,LCD_LINE4,"R");
TextOut(90,LCD_LINE5,"O");
TextOut(90,LCD_LINE6,"R");
while(true);
}
}
void DisplayGrid()
{
ClearScreen();
for(int r = 0; r < 8; r++)
for(int c = 0; c < 9; c++)
{
int d = 0;
switch(sudoku[r * 9 + c])
{
case ONE: d = 1; break;
case TWO: d = 2; break;
case THREE: d = 3; break;
case FOUR: d = 4; break;
case FIVE: d = 5; break;
case SIX: d = 6; break;
case SEVEN: d = 7; break;
case EIGHT: d = 8; break;
case NINE: d = 9; break;
}
if(d>0)
NumOut(c*7+18,56-r*7,d);
}
LineOut(17,16,79,16);
LineOut(17,40,79,40);
LineOut(17,63,79,63);
LineOut(16,0,16,63);
LineOut(37,0,37,63);
LineOut(59,0,59,63);
LineOut(80,0,80,63);
}
void ScanRow(int r,int direction)
{
int bmpOffset=r*SIZE_X;
if(direction==1)
{
for(int x=0;x<BEFORE;x+=1)
RotateMotorEx(OUT_B,SPEED_X,STEP_X,0,false,false);
for(int x=0;x<SIZE_X*3;x++)
{
byte light;
RotateMotorEx(OUT_B,SPEED_X,STEP_X,0,false,false);
light=Sensor(IN_2);
scanbuffer[x]=light;
}
}
else
{
for(int x=0;x<BEFORE;x+=1)
RotateMotorEx(OUT_B,SPEED_X,-1 * STEP_X,0,false,false);
for(int x=SIZE_X*3-1;x>=0;x--)
{
byte light;
RotateMotorEx(OUT_B,SPEED_X,-1 * STEP_X,0,false,false);
light=Sensor(IN_2);
scanbuffer[x]=light;
}
}
int maxLight=0;
int minLight=255;
for(int x=0;x<SIZE_X;x++)
{
int light=0;
for(int i=0;i<3;i++)
light+=scanbuffer[x*3+i];
light/=3;
bmp[bmpOffset+x]=light;
if(light>maxLight)
maxLight=light;
if(light<minLight)
minLight=light;
}
int threshold=(maxLight+minLight)/2;
for(int x=0;x<SIZE_X;x++)
if(bmp[bmpOffset+x]<threshold)
RectOut(18+x*2,r*2,1,1);
RotateMotorPID(OUT_B,-1 * direction * SPEED_X,SIZE_X*3*STEP_X+BEFORE*STEP_X,50,40,90);
}
void MoveSensor(int r, int c, int offsetX, int offsetY,bool scanCell)
{
int newMotorA;
int newMotorB;
Wait(100);
int oldMotorA=MotorRotationCount(OUT_A);
int oldMotorB=MotorRotationCount(OUT_B);
ResetTachoCount(OUT_AB);
int y=(r-4)*CELLHEIGHT;
newMotorB=sensorCellX[c] + offsetX;
newMotorA=sensorCellY[c] + y - offsetY -2520;
int DiffMotorB=newMotorB-oldMotorB;
int DiffMotorA=newMotorA-oldMotorA;
RotateMotorEx(OUT_B,50,DiffMotorB,0,false,true);
if(scanCell && DiffMotorA>0)
{
RotateMotorExPID(OUT_A,50,DiffMotorA+500,0,false,true,50,40,90);
RotateMotorExPID(OUT_A,50,-500,0,false,true,50,40,90);
}
else
RotateMotorExPID(OUT_A,50,DiffMotorA,0,false,true,50,40,90);
}
void ScanCell(int r, int c)
{
int direction;
int offset;
if(c<4)
direction=-1;
else
direction=1;
for(int i=0;i<SIZE_X*SIZE_Y;i++)
bmp[i]=0;
if(direction == 1)
offset=leftToRightOffset;
else
offset=rightToLeftOffset;
MoveSensor(r,c,-1*direction*(BEFORE*STEP_X +(SIZE_X*3*STEP_X)/2 - offset),-250,true);
TextOut(25,LCD_LINE1,"Scanning",true);
for(int y=0;y<SIZE_Y;y++)
{
RotateMotor(OUT_A, -1 * SPEED_Y,STEP_Y);
ScanRow(y,direction);
}
}
void DrawImage()
{
for(int y=0;y<SIZE_Y;y++)
{
RectOut(0,y*2,17,1);
for(int x=0;x<SIZE_X;x++)
{
if(image[y*SIZE_X+x]==0)
RectOut(18+x*2,y*2,1,1);
}
RectOut(82,y*2,17,1);
}
}
void Thresholding()
{
TextOut(10,LCD_LINE1,"Thresholding",true);
Wait(2000);
byte threshold;
long maxValue=0;
int whiteCount=0;
int blackCount=0;
byte min=bmp[0];
for(int i=1;i<SIZE_X*SIZE_Y;i++)
if(bmp[i] < min)
min = bmp[i];
for(byte t=min+2;whiteCount >= blackCount ;t++)
{
long whiteSum=0;
long blackSum=0;
whiteCount=0;
blackCount=0;
for(int i=0;i<SIZE_X*SIZE_Y;i++)
{
byte value=bmp[i];
if(value > t)
{
whiteCount++;
whiteSum+=value;
}
else
{
blackCount++;
blackSum+=value;
Pixel(i);
if(blackCount>SIZE_X*SIZE_Y/2)
break;
}
}
if(blackCount>30 && whiteCount>blackCount)
{
long blackMean=blackSum*10/blackCount;
long whiteMean=whiteSum*10/whiteCount;
long diff=whiteMean-blackMean;
whiteCount/=10;
blackCount/=10;
long value = diff*diff*whiteCount*blackCount;
if(value > maxValue)
{
maxValue=value;
threshold=t;
}
}
}
for(int i=0;i<SIZE_X*SIZE_Y;i++)
{
if(bmp[i] < threshold)
{
image[i] = 1;
//check if peak (holes in 6,8,9 maybe not recognized by threshold)
for(int j=i-1;j>0;j--)
if(bmp[j] > bmp[i])
break;
else if(bmp[i] - bmp[j] >= 2)
{
for(int k=i+1;k<SIZE_X*SIZE_Y;k++)
if(bmp[k] > bmp[i])
break;
else if(bmp[i] - bmp[k] >= 2)
{
image[i] = 0;
break;
}
}
}
else
image[i] = 0;
}
for(int i=0;i<SIZE_X;i++)
{
image[i]=0;
image[SIZE_X*SIZE_Y-SIZE_X+i]=0;
}
for(int i=0;i<SIZE_X*SIZE_Y;i+=SIZE_X)
{
image[i]=0;
image[SIZE_X-1+i]=0;
}
for(int i=SIZE_X;i<SIZE_X*SIZE_Y-SIZE_X;i++)
if(image[i-SIZE_X]==1 && image[i+SIZE_X]==1)
image[i]=1;
TextOut(10,LCD_LINE1,"Thresholding",true);
DrawImage();
}
void Segmentation()
{
TextOut(0,LCD_LINE1," Segmentation ");
int pos;
int middle=SIZE_X*SIZE_Y/2;
int stack[SIZE_X*SIZE_Y/2];
int stackSize=0;
for(int i=0;i<SIZE_X/2;i++)
{
pos=middle+i;
if(image[pos]==1)
break;
pos=middle-i;
if(image[pos]==1)
break;
}
stack[0]=pos;
while(stackSize>=0)
{
pos=stack[stackSize--];
for(int i=-1;i<2;i++)
for(int j=-1;j<2;j++)
{
int p=pos+i*SIZE_X+j;
if(image[p]==1)
{
stack[++stackSize]=p;
image[p]=2;
}
}
}
for(int i=0;i<SIZE_X*SIZE_Y;i++)
if(image[i]==2)
image[i]=1;
else
image[i]=0;
DrawImage();
}
void Thinning()
{
TextOut(0,LCD_LINE1," Thinning ");
for(int y=0;y<SIZE_Y-2;y++)
{
RectOut(0,y*2,17,1);
for(int x=0;x<SIZE_X;x++)
{
if(image[y*SIZE_X+x]==0)
RectOut(18+x*2,y*2,1,1);
}
RectOut(82,y*2,17,1);
}
bool mark[SIZE_X*SIZE_Y];
for(int i=0;i<SIZE_X*SIZE_Y;i++)
mark[i]=false;
int marked=1;
while(marked>0)
{
marked=0;
for(int i=SIZE_X+1;i<SIZE_X*SIZE_Y-SIZE_X-1;i++)
if(image[i])
{
byte p1=image[i-SIZE_X];
byte p2=image[i-SIZE_X+1];
byte p3=image[i+1];
byte p4=image[i+SIZE_X+1];
byte p5=image[i+SIZE_X];
byte p6=image[i+SIZE_X-1];
byte p7=image[i-1];
byte p8=image[i-SIZE_X-1];
byte n=p1+p2+p3+p4+p5+p6+p7+p8;
byte s=0;
if(p1==0 && p2==1) s++;
if(p2==0 && p3==1) s++;
if(p3==0 && p4==1) s++;
if(p4==0 && p5==1) s++;
if(p5==0 && p6==1) s++;
if(p6==0 && p7==1) s++;
if(p7==0 && p8==1) s++;
if(p8==0 && p1==1) s++;
if(n>=2 && s==1 && p1*p3*p5==0 && p3*p5*p7==0)
{
mark[i]=true;
marked++;
}
}
for(int i=0;i<SIZE_X*SIZE_Y;i++)
if(mark[i])
{
image[i]=0;
mark[i]=0;
Pixel(i);
}
for(int i=SIZE_X+1;i<SIZE_X*SIZE_Y-SIZE_X-1;i++)
if(image[i])
{
byte p1=image[i-SIZE_X];
byte p2=image[i-SIZE_X+1];
byte p3=image[i+1];
byte p4=image[i+SIZE_X+1];
byte p5=image[i+SIZE_X];
byte p6=image[i+SIZE_X-1];
byte p7=image[i-1];
byte p8=image[i-SIZE_X-1];
byte n=p1+p2+p3+p4+p5+p6+p7+p8;
byte s=0;
if(p1==0 && p2==1) s++;
if(p2==0 && p3==1) s++;
if(p3==0 && p4==1) s++;
if(p4==0 && p5==1) s++;
if(p5==0 && p6==1) s++;
if(p6==0 && p7==1) s++;
if(p7==0 && p8==1) s++;
if(p8==0 && p1==1) s++;
if(n>=2 && s==1 && p1*p3*p7==0 && p1*p5*p7==0)
{
mark[i]=true;
marked++;
}
}
for(int i=0;i<SIZE_X*SIZE_Y;i++)
if(mark[i])
{
image[i]=0;
mark[i]=0;
Pixel(i);
}
}
for(int i=SIZE_X+1;i<SIZE_X*SIZE_Y-SIZE_X-1;i++)
if(image[i])
{
byte p1=image[i-SIZE_X];
byte p2=image[i-SIZE_X+1];
byte p3=image[i+1];
byte p4=image[i+SIZE_X+1];
byte p5=image[i+SIZE_X];
byte p6=image[i+SIZE_X-1];
byte p7=image[i-1];
byte p8=image[i-SIZE_X-1];
byte n=p1+p2+p3+p4+p5+p6+p7+p8;
byte s=0;
if(p1==0 && p2==0 && p3==1) s++;
if(p3==0 && p4==0 && p5==1) s++;
if(p5==0 && p6==0 && p7==1) s++;
if(p7==0 && p8==0 && p1==1) s++;
if(p1==0 && p2==1) s++;
if(p3==0 && p4==1) s++;
if(p5==0 && p6==1) s++;
if(p7==0 && p8==1) s++;
if(n>=2 && s==1)
{
image[i]=0;
Pixel(i);
}
}
}
int Neighbor(int pos, int prevPos,int &nextPos)
{
if(pos!=prevPos && image[pos])
{
nextPos=pos;
return 1;
}
else
return 0;
}
int TipLen(int pos)
{
int prevPos=-1;
int nextPos;
int n;
int length=0;
do
{
if(++length > 5)
break;
n=Neighbor(pos-SIZE_X,prevPos,nextPos);
n+=Neighbor(pos-SIZE_X+1,prevPos,nextPos);
n+=Neighbor(pos+1,prevPos,nextPos);
n+=Neighbor(pos+SIZE_X+1,prevPos,nextPos);
n+=Neighbor(pos+SIZE_X,prevPos,nextPos);
n+=Neighbor(pos+SIZE_X-1,prevPos,nextPos);
n+=Neighbor(pos-1,prevPos,nextPos);
n+=Neighbor(pos-SIZE_X-1,prevPos,nextPos);
prevPos=pos;
pos=nextPos;
}
while(n==1)
return length;
}
int Tip(int pos,bool &longTip)
{
byte p1=image[pos-SIZE_X];
byte p2=image[pos-SIZE_X+1];
byte p3=image[pos+1];
byte p4=image[pos+SIZE_X+1];
byte p5=image[pos+SIZE_X];
byte p6=image[pos+SIZE_X-1];
byte p7=image[pos-1];
byte p8=image[pos-SIZE_X-1];
byte n=p1+p2+p3+p4+p5+p6+p7+p8;
if(n==1)
{
int tipLen=TipLen(pos);
longTip=tipLen > 5;
if(tipLen==1)
return 0;
else if(p1) return(NORTH);
else if(p2) return(NORTHWEST);
else if(p3) return(WEST);
else if(p4) return(SOUTHWEST);
else if(p5) return(SOUTH);
else if(p6) return(SOUTHEAST);
else if(p7) return(EAST);
else if(p8) return(NORTHEAST);
}
else
return 0;
}
void Recognization(int r, int c)
{
int topPixel;
int botPixel=0;
int leftPixel=SIZE_X;
int rightPixel=0;
for(int i=0;i<SIZE_X*SIZE_Y;i++)
if(image[i])
{
if(!botPixel)
botPixel=i;
topPixel=i;
int col=i%SIZE_X;
if(col<leftPixel)
leftPixel=col;
if(col>rightPixel)
rightPixel=col;
}
int topRow=topPixel/SIZE_X;
int botRow=botPixel/SIZE_X;
int width=rightPixel-leftPixel+1;
int longTipUpper=0;
int longTipLower=0;
int leftTips=0;
int topTip=0;
int botTip=0;
for(int i=0;i<SIZE_X*SIZE_Y;i++)
if(image[i])
{
bool longTip=false;
int tip=Tip(i,longTip);
if(tip)
{
if(longTip)
{
if(!longTipLower)
longTipLower=tip;
else if(!longTipUpper)
longTipUpper=tip;
}
if(tip==WEST || tip==NORTHWEST || tip==SOUTHWEST)
leftTips++;
if(i/SIZE_X==topRow)
topTip=tip;
if(i/SIZE_X==botRow)
botTip=tip;
}
}
int digit;
if(width<6)
digit=1;
else if(leftTips==3 ||((longTipUpper == WEST||longTipUpper==NORTHWEST) && (longTipLower==WEST ||longTipLower==NORTHWEST) && botTip==0))
digit=3;
else if((longTipUpper==WEST||longTipUpper==NORTHWEST||longTipUpper==SOUTHWEST) && (longTipLower==WEST||longTipLower==SOUTHWEST||longTipLower==SOUTH))
digit=7;
else if(leftTips && (longTipLower==EAST||longTipLower==SOUTHEAST||longTipLower==NORTHEAST) && topTip!=NORTHEAST && topTip!=EAST)
digit=2;
else if((longTipUpper==EAST||longTipUpper==NORTHEAST||longTipUpper==SOUTHEAST) && (longTipLower==WEST||longTipLower==SOUTHWEST||longTipLower==NORTHWEST))
digit=5;
else if(topTip==NORTH||topTip==NORTHEAST||topTip==EAST)
digit=6;
else if(botTip==SOUTH||botTip==SOUTHWEST||botTip==WEST || longTipLower==WEST||longTipLower==SOUTHWEST||longTipLower==SOUTH)
digit=9;
else if((topTip==NORTH||topTip==NORTHEAST||topTip==NORTHWEST) &&(botTip==SOUTH||botTip==SOUTHWEST||botTip==SOUTHEAST))
digit=4;
else
digit=8;
NumOut(45,LCD_LINE5,digit,true);
string soundFile;
string ds=NumToStr(digit);
soundFile=StrCat("0",ds,".rso");
PlayFile(soundFile);
sudoku[r*9 + c] = 1<<(digit-1);
}
void ScanSudoku()
{
FindNonEmptyCells();
for(int r=0;r<9;r++)
for(int c=0;c<9;c++)
if(probe[r*9+c])
{
ScanCell(r,c);
Thresholding();
Segmentation();
Thinning();
Recognization(r,c);
}
SetSensorType(IN_2,SENSOR_TYPE_NONE);
DisplayGrid();
}
void EliminateRow(int r, int c, int digits)
{
for (int i = r*9; i < r*9 + 9; i += 3)
if((i % 9) != c)
for(int j = 0; j < 3; j++)
eliminatedDigitsInCell[i + j] |= digits;
}
void EliminateCol(int r, int c, int digits)
{
for (int i = c ; i < c+ 81 ; i += 3*9)
if((i / 9) != r)
for(int j = 0; j < 3*9; j+=9)
eliminatedDigitsInCell[i + j] |= digits;
}
void InitSolve()
{
for(int i = 0; i < 81; i++)
if(sudoku[i] != 0)
{
int r = i / 9;
int c = i % 9;
int b = r / 3 * 3 + c / 3;
eliminatedDigitsInRow[r] |= sudoku[i];
eliminatedDigitsInCol[c] |= sudoku[i];
eliminatedDigitsInBox[b] |= sudoku[i];
}
for(int i = 0; i < 81; i++)
{
int r = i / 9;
int c = i % 9;
int b = r / 3 * 3 + c / 3;
if(sudoku[i] != 0)
eliminatedDigitsInCell[i] = 511;
else
eliminatedDigitsInCell[i] = eliminatedDigitsInRow[r] | eliminatedDigitsInCol[c] | eliminatedDigitsInBox[b];
}
for(int r = 0; r < 9; r+=3)
for(int c = 0; c < 9; c+=3)
{
int i=r*9+c;
int r0 = eliminatedDigitsInCell[i] & eliminatedDigitsInCell[i + 1] & eliminatedDigitsInCell[i + 2];
int r1 = eliminatedDigitsInCell[i + 9] & eliminatedDigitsInCell[i + 10] & eliminatedDigitsInCell[i + 11];
int r2 = eliminatedDigitsInCell[i + 18] & eliminatedDigitsInCell[i + 19] & eliminatedDigitsInCell[i + 20];
int c0 = eliminatedDigitsInCell[i] & eliminatedDigitsInCell[i + 9] & eliminatedDigitsInCell[i + 18];
int c1 = eliminatedDigitsInCell[i+1] & eliminatedDigitsInCell[i + 10] & eliminatedDigitsInCell[i + 19];
int c2 = eliminatedDigitsInCell[i + 2] & eliminatedDigitsInCell[i + 11] & eliminatedDigitsInCell[i + 20];
if((r0^511) & r1 & r2) //r0^511 because ~r0 isn't supported in standard firmware
EliminateRow(r, c, (r0^511) & r1 & r2);
if(r0 & (r1^511) & r2)
EliminateRow(r+1, c, r0 & (r1^511) & r2);
if(r0 & r1 & (r2^511))
EliminateRow(r+2, c, r0 & r1 & (r2^511));
if((c0^511) & c1 & c2)
EliminateCol(r, c, (c0^511) & c1 & c2);
if(c0 & (c1^511) & c2)
EliminateCol(r, c+1, c0 & (c1^511) & c2);
if(c0 & c1 & (c2^511))
EliminateCol(r, c+2, c0 & c1 & (c2^511));
}
for(int i = 0; i < 512; i++)
{
bitCount[i] = 0;
for(int j = 1; j <= 256; j *= 2)
if (i & j)
bitCount[i]++;
}
}
int GetCell()
{
int maxDigits = 0;
int bestCell = -1;
int i = 0;
for(int r = 0; r < 9; r++)
for(int c = 0; c < 9; c++)
{
if(!sudoku[i])
{
int b = r / 3 * 3 + c / 3;
int eliminatedDigits = eliminatedDigitsInRow[r] | eliminatedDigitsInCol[c] | eliminatedDigitsInBox[b] | eliminatedDigitsInCell[i];
if(bitCount[eliminatedDigits] > maxDigits)
{
maxDigits = bitCount[eliminatedDigits];
bestCell = i;
if(maxDigits == 8)
return bestCell;
}
}
i++;
}
return bestCell;
}
void NextLevel(int level)
{
int r = cell[level] / 9;
int c = cell[level] % 9;
int b = r / 3 * 3 + c / 3;
row[level] = eliminatedDigitsInRow[r];
col[level] = eliminatedDigitsInCol[c];
box[level] = eliminatedDigitsInBox[b];
eliminatedDigitsInRow[r] |= digit[level];
eliminatedDigitsInCol[c] |= digit[level];
eliminatedDigitsInBox[b] |= digit[level];
}
void PreviousLevel(int level)
{
int r = cell[level] / 9;
int c = cell[level] % 9;
int b = r / 3 * 3 + c / 3;
eliminatedDigitsInRow[r]=row[level];
eliminatedDigitsInCol[c]=col[level];
eliminatedDigitsInBox[b]=box[level];
}
void SolveSudoku()
{
int level=0;
InitSolve();
cell[0]=GetCell();
digit[level]=512;
while(true)
{
digit[level] /= 2;
if(!digit[level])
{
sudoku[cell[level]] = 0;
PreviousLevel(--level);
}
else
{
int r = cell[level] / 9;
int c = cell[level] % 9;
int b = r / 3 * 3 + c / 3;
if(!((eliminatedDigitsInRow[r] | eliminatedDigitsInCol[c] | eliminatedDigitsInBox[b]| eliminatedDigitsInCell[cell[level]]) & digit[level]))
{
sudoku[cell[level]] = digit[level];
NextLevel(level++);
cell[level] = GetCell();
if (cell[level] < 0)
break;
digit[level] = 512;
PlayTone(200+level*20,1);
}
}
}
DisplayGrid();
}
void MovePen(int r, int c, int offsetX, int offsetY)
{
int newMotorA;
int newMotorB;
Wait(100);
int oldMotorA=MotorRotationCount(OUT_A);
int oldMotorB=MotorRotationCount(OUT_B);
ResetTachoCount(OUT_AB);
int y=(r-4)*CELLHEIGHT;
newMotorB=penCellX[c] + offsetX;
newMotorA=penCellY[c] + y - offsetY - 600;
int DiffMotorB=newMotorB-oldMotorB;
int DiffMotorA=newMotorA-oldMotorA;
RotateMotorEx(OUT_B,50,DiffMotorB,0,false,true);
if(DiffMotorA<0)
{
RotateMotorExPID(OUT_A,50,DiffMotorA-200,0,false,true,50,40,90);
RotateMotorExPID(OUT_A,50,+200,0,false,true,50,40,90);
}
else
RotateMotorExPID(OUT_A,50,DiffMotorA,0,false,true,50,40,90);
}
void WriteDigit(int digit, int row, int col)
{
int xStep=5;
int yStep=15;
int startX,startY;
string sequence;
switch(digit)
{
case ONE:
sequence="DDDDDDDDDDDDDDD";
startX=0;
startY=6;
break;
case TWO:
sequence="RRRRRRRRRRDDDDDDDLLLLLLLLLLLDDDDDDDRRRRRRRRRRRR";
startX=-4;
startY=6;
break;
case THREE:
sequence="RRRRRRRRRRDDDDDDDLLLLLLLLLLRRRRRRRRRRDDDDDDDLLLLLLLLLLLL";
startX=-4;
startY=6;
break;
case FOUR:
sequence="DDDDDDDDURRRRRRRRRLLLLUUUUUUUUDDDDDDDDDDDDDDD";
startX=-4;
startY=6;
break;
case FIVE:
sequence="LLLLLLLLLLRDDDDDDDURRRRRRRRDRRDDRDLLDDLLDLDLLLLLUULLUL";
startX=4;
startY=6;
break;
case SIX:
sequence="LLLLLLLLLDDDDDDDDDDDDDRRRRRRRRRRRUUUUUUULLLLLLLLLLL";
startX=4;
startY=6;
break;
case SEVEN:
sequence="RRRRRRRRRRLLDLDLDDLDDLDDLDDLDDLDDLD";
startX=-4;
startY=6;
break;
case EIGHT:
sequence="RRRRRRRRDDDDDDDLLLLLLLLLLRRRRRRRRRRDDDDDDDLLLLLLLLLLUUUUUUUUUUUUUU";
startX=-3;
startY=6;
break;
case NINE:
sequence="LLLLLLLLLUUUUUUURRRRRRRRRRRDDDDDDDDDDDDDDLLLLLLLLLL";
startX=4;
startY=0;
break;
}
MovePen(row,col,startX * xStep,startY*yStep);
PenDown();
for(int i=0;i<StrLen(sequence);i++)
{
char direction=StrIndex(sequence,i);
if(direction=='L')
RotateMotorEx(OUT_B, PENSPEED_X,-1 * xStep,0,false,false);
else if(direction=='R')
RotateMotorEx(OUT_B, PENSPEED_X,xStep,0,false,false);
else if(direction=='U')
RotateMotorEx(OUT_A, PENSPEED_Y,-1*yStep,0,false,false);
else if(direction=='D')
RotateMotorEx(OUT_A, PENSPEED_Y,yStep,0,false,false);
}
Off(OUT_AB);
PenUp();
}
void PrintSudoku()
{
for(int c=0;c<9;c++)
for(int r=0;r<9;r++)
if(!probe[r * 9 + c])
WriteDigit(sudoku[r * 9 + c],r,c);
PlayFile("Game Over.rso");
Wait(3000);
}
task main()
{
Init();
ScanSudoku();
SolveSudoku();
PrintSudoku();
}
|
|
楼主: foxlover
|手机版|中文乐高
( 桂ICP备13001575号-7 )
IP卡
狗仔卡
显身卡
