/* 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;imaxLight) 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 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 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= 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=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;xmaxLight) maxLight=light; if(light0) { 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= blackCount ;t++) { long whiteSum=0; long blackSum=0; whiteCount=0; blackCount=0; for(int i=0;i 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;i0;j--) if(bmp[j] > bmp[i]) break; else if(bmp[i] - bmp[j] >= 2) { for(int k=i+1;k bmp[i]) break; else if(bmp[i] - bmp[k] >= 2) { image[i] = 0; break; } } } else image[i] = 0; } for(int i=0;i=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;i0) { marked=0; for(int i=SIZE_X+1;i=2 && s==1 && p1*p3*p5==0 && p3*p5*p7==0) { mark[i]=true; marked++; } } for(int i=0;i=2 && s==1 && p1*p3*p7==0 && p1*p5*p7==0) { mark[i]=true; marked++; } } for(int i=0;i=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;irightPixel) 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 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