8547解魔方程序1----校准并扫描

jackessence

这个是我该写的原来hans先生的程序，现在利用了新的颜色传感器，实际上程序比原来的还要简单
代码
#define LEFTFACE      0
#define FRONTFACE     1
#define RIGHTFACE     2
#define BACKFACE      3
#define UPPERFACE     4
#define DOWNFACE      5
#define COLOR_SENSOR IN_2
#define OFFSETC       35
int tachoCenter;  //转动中心
int color[9]={0,0,0,0,0,0,0,0,0};
int raw[3]={0,0,0};
int norm[3]={0,0,0};
int scaled[3]={0,0,0};
void Tilt()
{
  RotateMotorEx(OUT_B,50,40,0,false,false);
  RotateMotorEx(OUT_B,100,15,0,false,false);
  Wait(800);
  RotateMotor(OUT_B,40,10);
  RotateMotor(OUT_B,30,-65);
}
void TurnQuarter(bool coast)
{
  RotateMotorExPID(OUT_A,100,315,false,false,!coast,40,40,90);
}
int ReadColor(int face)
{
int colorval;
ReadSensorColorEx(COLOR_SENSOR, colorval, raw, norm, scaled);
return colorval;
}

void ScanFace(int face, string title, int preTurn, int postTurn)
{
  TextOut(20,LCD_LINE2,title,true);
//Center
  RotateMotor(OUT_C,20,tachoCenter - OFFSETC);
  color[4]=ReadColor(face);
//Edges
  RotateMotor(OUT_C,20,-20);
  RotateMotorExPID(OUT_A,100,60+preTurn*315,false,false,true,40,40,90);
  color[1]=ReadColor(face);
  //TurnQuarter(!colorSensor);
  TurnQuarter(false);
  color[3]=ReadColor(face);
  //TurnQuarter(!colorSensor);
  TurnQuarter(false);
  color[7]=ReadColor(face);
  //TurnQuarter(false);
  TurnQuarter(false);
  color[5]=ReadColor(face);
//Corners
  RotateMotor(OUT_C,20,-10);
  RotateMotorExPID(OUT_A,100,160,false,false,true,40,40,90);
  color[0]=ReadColor(face);
  TurnQuarter(false);
  color[2]=ReadColor(face);
  TurnQuarter(false);
  color[8]=ReadColor(face);
  TurnQuarter(false);
  color[6]=ReadColor(face);
  RotateMotorPID(OUT_A,100,95+postTurn*315,40,40,90);
  RotateMotor(OUT_C,20,-1 * tachoCenter + 30 + OFFSETC);
}
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,10);
  until(SENSOR_1 == 1);
  Off(OUT_C);
  tachoCenter = abs(MotorTachoCount(OUT_C));
  NumOut(30,LCD_LINE3,tachoCenter);
  RotateMotor(OUT_C,10,OFFSETC);
  Wait(2000);
}
void Initialize()
{
  SetSensorColorFull(IN_2);
  TextOut(0,LCD_LINE1,"color sensor");  
  SetSensorLowspeed(IN_3);
  SetSensorTouch(IN_1);
}

task main()
{
  Initialize();
  CalibrateSensorPosition();
  
  ScanFace(LEFTFACE,"LEFT FACE",0,0);
  NumOut(10,LCD_LINE3,color[0]);
  NumOut(20,LCD_LINE3,color[1]);
  NumOut(30,LCD_LINE3,color[2]);
  NumOut(10,LCD_LINE4,color[3]);
  NumOut(20,LCD_LINE4,color[4]);
  NumOut(30,LCD_LINE4,color[5]);
  NumOut(10,LCD_LINE5,color[6]);
  NumOut(20,LCD_LINE5,color[7]);
  NumOut(30,LCD_LINE5,color[8]);
  Wait(3000);
  
  
  Tilt();
  ScanFace(BACKFACE,"BACK FACE",1,0);
  NumOut(10,LCD_LINE3,color[0]);
  NumOut(20,LCD_LINE3,color[1]);
  NumOut(30,LCD_LINE3,color[2]);
  NumOut(10,LCD_LINE4,color[3]);
  NumOut(20,LCD_LINE4,color[4]);
  NumOut(30,LCD_LINE4,color[5]);
  NumOut(10,LCD_LINE5,color[6]);
  NumOut(20,LCD_LINE5,color[7]);
  NumOut(30,LCD_LINE5,color[8]);
  Wait(3000);
  
  
  Tilt();
  ScanFace(RIGHTFACE,"RIGHT FACE",1,3);
  NumOut(10,LCD_LINE3,color[0]);
  NumOut(20,LCD_LINE3,color[1]);
  NumOut(30,LCD_LINE3,color[2]);
  NumOut(10,LCD_LINE4,color[3]);
  NumOut(20,LCD_LINE4,color[4]);
  NumOut(30,LCD_LINE4,color[5]);
  NumOut(10,LCD_LINE5,color[6]);
  NumOut(20,LCD_LINE5,color[7]);
  NumOut(30,LCD_LINE5,color[8]);
  Wait(3000);
  
  
  Tilt();
  ScanFace(DOWNFACE,"DOWN FACE",1,1);
  NumOut(10,LCD_LINE3,color[0]);
  NumOut(20,LCD_LINE3,color[1]);
  NumOut(30,LCD_LINE3,color[2]);
  NumOut(10,LCD_LINE4,color[3]);
  NumOut(20,LCD_LINE4,color[4]);
  NumOut(30,LCD_LINE4,color[5]);
  NumOut(10,LCD_LINE5,color[6]);
  NumOut(20,LCD_LINE5,color[7]);
  NumOut(30,LCD_LINE5,color[8]);
  Wait(3000);
  
  
  Tilt();
  ScanFace(FRONTFACE,"FRONT FACE",0,1);
  NumOut(10,LCD_LINE3,color[0]);
  NumOut(20,LCD_LINE3,color[1]);
  NumOut(30,LCD_LINE3,color[2]);
  NumOut(10,LCD_LINE4,color[3]);
  NumOut(20,LCD_LINE4,color[4]);
  NumOut(30,LCD_LINE4,color[5]);
  NumOut(10,LCD_LINE5,color[6]);
  NumOut(20,LCD_LINE5,color[7]);
  NumOut(30,LCD_LINE5,color[8]);
  Wait(3000);
  
  
  Tilt();
  ScanFace(UPPERFACE,"UPPER FACE",0,0);
  NumOut(10,LCD_LINE3,color[0]);
  NumOut(20,LCD_LINE3,color[1]);
  NumOut(30,LCD_LINE3,color[2]);
  NumOut(10,LCD_LINE4,color[3]);
  NumOut(20,LCD_LINE4,color[4]);
  NumOut(30,LCD_LINE4,color[5]);
  NumOut(10,LCD_LINE5,color[6]);
  NumOut(20,LCD_LINE5,color[7]);
  NumOut(30,LCD_LINE5,color[8]);
  Wait(3000);
}

Programus

貌似你漏掉了一个写文件的部分。
这个校准程序其实是把各个面的颜色值读取后取一个平均值，写入文件中。
这个文件将在实际还原魔方的程序中读取，作为颜色判断基准。
另外，这个文件中还记录了颜色传感器的复位位置到魔方顶端中心块位置的转角。

文件蛮重要的哟，你看看是不是漏掉了，还是忘了贴上来了。

jackessence

2# Programus


    哈哈，您看得很仔细，主要是当时我在考虑颜色的问题，所以没有写这一段，我现在的程序是只校正位置的那个值，颜色的不必较准，只是做个测试而已，不写入文件，我采用的最终方法是colorval返回颜色值，当然是1，2，3，4，5，6中的一个，然后用一个switch语句把他们解释为对应的rgb值，这几个rgb采用的是作者程序里的那些light传感器测出的值，我试试一下，好像可以，不过机器人没电了，很讨厌，还要充好电才能检验到底行不行

Programus

Programus


    哈哈，您看得很仔细，主要是当时我在考虑颜色的问题，所以没有写这一段，我现在的程序是只校正位置的那个值，颜色的不必较准，只是做个测试而已，不写入文件，我采用的最终方法是colorval返回颜色值，当然是1，2，3，4，5，6中的一个，然后用一个switch语句把他们解释为对应的rgb值，这几个rgb采用的是作者程序里的那些light传感器测出的值，我试试一下，好像可以，不过机器人没电了，很讨厌，还要充好电才能检验到底行不行
jackessence 发表于 2009-10-7 21:01

我觉得写入文件可以简化后面的复原步骤。因为魔方因为品种的不同，每个颜色的相对位置也会不同。如果在校准的时候写入文件，复原的时候就不必去考虑颜色的相对位置了，直接使用文件中存储的结果即可。不过，这一步算是个小问题，即便没有，对复原程序的影响也不是很大。
祝你早日试验成功！

ps. NXT确实太费电了，我一直在考虑怎么给NXT改造成可以用变压器的，不过没舍得下手……

jackessence

4# Programus


    哈哈哈，问题基本全部解决，我会把经验发上来的

Programus

好啊！期待……

qinxd99

很不错

s185

两个牛人之间的对话 菜鸟完全看不懂

1234

?????