privatestaticLinkedList<String>pushChain=newLinkedList<String>(),rotateChain=newLinkedList<String>();static{pushChain.add(D);pushChain.add(F);pushChain.add(U);pushChain.add(B);rotateChain.add(F);rotateChain.add(L);rotateChain.add(B);rotateChain.add(R);}/** * push the cube using the long arm. * Face FRONT turns to DOWN */privatestaticvoidpush(){arm.rotate(PUSH_ANGLE);sleep(50);arm.rotate(-PUSH_ANGLE);// update pushChain and rotateChainpushChain.add(pushChain.remove(0));rotateChain.set(0,pushChain.get(1));rotateChain.set(2,pushChain.get(3));}/** * rotate the base (DOWN FACE) with a specified angle * @param angle the degrees to rotate * @param changeFacelet whether to update the facelet chain */privatestaticvoidrotate(intangle,booleanchangeFacelet){intcurrentPosition=base.getTachoCount();switch(angle){case90:if(currentPosition>225){base.rotateTo(currentPosition-270);}else{base.rotateTo(currentPosition+90);}if(changeFacelet)rotateChain.add(rotateChain.remove(0));break;case180:if(currentPosition>135){base.rotateTo(currentPosition-180);}else{base.rotateTo(currentPosition+180);}if(changeFacelet){rotateChain.add(rotateChain.remove(0));rotateChain.add(rotateChain.remove(0));}break;case-90:if(currentPosition<45){base.rotateTo(currentPosition+270);}else{base.rotateTo(currentPosition-90);}if(changeFacelet)rotateChain.add(0,rotateChain.remove(3));break;default:}if(changeFacelet){pushChain.set(1,rotateChain.get(0));pushChain.set(3,rotateChain.get(2));}}
/** * make the specified facelet downwards to bottom * @param facelet target face */publicstaticvoidchangetoFacelet(Stringfacelet){switch(pushChain.indexOf(facelet)){case0:return;case1:push();return;case2:push();push();return;case3:rotate(180,true);push();return;default:}switch(rotateChain.indexOf(facelet)){case1:rotate(90,true);push();return;case3:rotate(-90,true);push();return;default:}}
拧(TWIST)的动作可以通过HOLD->ROTATE->RELEASE的步骤实现
1234567891011121314151617181920212223242526
/** * twist DOWN FACE 90 degrees */privatestaticvoidturnClockwise(){hold();rotate(90,false);release();}/** * twist DOWN FACE -90 degrees */privatestaticvoidturnAntiClockwise(){hold();rotate(-90,false);release();}/** * twist DOWN FACE 180 degrees */privatestaticvoidturnSemiCycle(){hold();rotate(180,false);release();}
至此,输入公式,机器人就可以一步步操作了
12345678910111213141516171819
/** * Execute a specified expression * @param exp the expression to be executed */staticvoidexecute(Stringexp){for(inti=0;i<exp.length();i++){changetoFacelet(exp.substring(i,i+1));if(i==exp.length()-1||exp.charAt(i+1)==' '){turnClockwise();i++;}elseif(exp.charAt(i+1)=='\''){turnAntiClockwise();i+=2;}elseif(exp.charAt(i+1)=='2'){turnSemiCycle();i+=2;}}}
-(void)handlePacketWithType:(uint8_t)packet_typeforChannel:(uint16_t)channelandData:(uint8_t*)packetwithLen:(uint16_t)size{switch(state){// -- omitted --casekActivated:
switch(packet_type){caseHCI_EVENT_PACKET:
switch(packet[0]){caseBTSTACK_EVENT_STATE:
[selfactivationHandleEvent:packetwithLen:size];break;caseRFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
if(packet[2]){printf("RFCOMM channel open failed, status %u\n",packet[2]);// TODO connection failed callback}else{uint16_trfcomm_channel_id=READ_BT_16(packet,12);uint16_tmtu=READ_BT_16(packet,14);printf("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u\n",rfcomm_channel_id,mtu);[self.rfcommDelegaterfcommConnectionCreatedAtAddress:""forChannel:channelasID:rfcomm_channel_id];}break;caseHCI_EVENT_DISCONNECTION_COMPLETE:
printf("Basebank connection closed\n");uint16_trfcomm_channel_id=READ_BT_16(packet,12);[self.rfcommDelegaterfcommConnectionClosedForConnectionID:rfcomm_channel_id];break;default:break;}break;caseRFCOMM_DATA_PACKET:
NSLog(@"Received from 0x%02X %@",channel,[NSDatadataWithBytes:packetlength:size]);[self.rfcommDelegaterfcommDataReceivedForConnectionID:channelwithData:packetofLen:size];break;}[selfdiscoveryHandleEvent:packetwithLen:size];break;default:break;}// -- omitted --}-(BTstackError)createRFCOMMConnectionAtAddress:(bd_addr_t*)addresswithChannel:(uint16_t)channelauthenticated:(BOOL)authentication{if(state<kActivated)returnBTSTACK_NOT_ACTIVATED;if(state!=kActivated)returnBTSTACK_BUSY;bt_send_cmd(&rfcomm_create_channel,address,channel);return0;};-(BTstackError)sendRFCOMMPacket:(NSData*)packetForConnectionId:(uint16_t)connectionId{if(state<kActivated)returnBTSTACK_NOT_ACTIVATED;NSLog(@"Send to 0x%02X %@",connectionId,packet);bt_send_rfcomm(connectionId,(uint8_t*)[packetbytes],[packetlength]);return0;};-(BTstackError)closeRFCOMMConnectionWithID:(uint16_t)connectionID{if(state<kActivated)returnBTSTACK_NOT_ACTIVATED;bt_send_cmd(&hci_disconnect,connectionID);return0;};
publicvoidwrite(byte[]data)throwsIOException{// Send length of packet (Least and Most significant byte)// * NOTE: Bluetooth only. os.write((byte)data.length);os.write((byte)(data.length>>>8));os.write(data);os.flush();}
voidfindSquares(Mat&image,vector<vector<cv::Point>>&squares){squares.clear();vector<vector<cv::Point>>contours;// find contours and store them all as a listfindContours(image,contours,CV_RETR_LIST,CV_CHAIN_APPROX_SIMPLE);vector<cv::Point>approx;// test each contourfor(size_ti=0;i<contours.size();i++){// approximate contour with accuracy proportional// to the contour perimeterapproxPolyDP(Mat(contours[i]),approx,arcLength(Mat(contours[i]),true)*0.05,true);// square contours should have 4 vertices after approximation// relatively large area (to filter out noisy contours)// and be convex.// Note: absolute value of an area is used because// area may be positive or negative - in accordance with the// contour orientationif(approx.size()==4&&fabs(contourArea(Mat(approx)))>1000&&isContourConvex(Mat(approx))){doublemaxCosine=0;for(intj=2;j<5;j++){// find the maximum cosine of the angle between joint edgesdoublecosine=fabs(angle(approx[j%4],approx[j-2],approx[j-1]));maxCosine=MAX(maxCosine,cosine);}doublelineLength1=distance(approx[0],approx[1]);doublelineLength2=distance(approx[1],approx[2]);// if cosines of all angles are small and border length almost equals// (all angles are ~90 degree) then write quandrange// vertices to resultant sequence// then filter out big squares such as the whole facelet of the cubeif(maxCosine<0.1&&fabs(lineLength1-lineLength2)/MAX(lineLength1,lineLength2)<0.1&&lineLength1/image.cols<0.3){if(squares.empty()){squares.push_back(approx);}else{// make sure no overlapfor(vector<vector<cv::Point>>::iterators=squares.begin();s<squares.end();s++){intcontains=squareContains(*s,approx);if(contains==1){//s contains approxsquares.erase(s);squares.push_back(approx);break;}elseif(contains==-1){// approx contains sbreak;// discard this approx}else{if(s==squares.end()-1){squares.push_back(approx);break;}}}}}}}// printf("found %lu squares\n", squares.size());if(squares.size()!=9){squares.clear();}else{// sort squares to sequence// 0 1 2// 3 4 5// 6 7 8sort(squares.begin(),squares.end(),compareTwoPointsWithY);sort(squares.begin()+0,squares.begin()+3,compareTwoPointsWithX);sort(squares.begin()+3,squares.begin()+6,compareTwoPointsWithX);sort(squares.begin()+6,squares.begin()+9,compareTwoPointsWithX);}}
-(NSString*)detectColors{sort(colorNodes.begin(),colorNodes.end(),sortForWhite);// W X X X X Xsort(colorNodes.begin()+9,colorNodes.end(),sortForGreen);// W G X X X Xsort(colorNodes.begin()+18,colorNodes.end(),sortForBlue);// W G B R O Ycharcolors[]="WWWWWWWWWGGGGGGGGGBBBBBBBBBRRRRRRRRROOOOOOOOOYYYYYYYYY";for(inti=0;i<colorNodes.size();i++){colorNodes[i].color=colors[i];}sort(colorNodes.begin(),colorNodes.end(),restoreInputSequence);map<char,char>colorMap;colorMap[colorNodes[4].color]='U';colorMap[colorNodes[13].color]='B';colorMap[colorNodes[22].color]='D';colorMap[colorNodes[31].color]='F';colorMap[colorNodes[40].color]='R';colorMap[colorNodes[49].color]='L';intconvertTable[]={8,7,6,5,4,3,2,1,0,38,41,44,37,40,43,36,39,42,35,34,33,32,31,30,29,28,27,26,25,24,23,22,21,20,19,18,47,50,53,46,49,52,45,48,51,9,10,11,12,13,14,15,16,17};charstate[55];state[54]=0;printf("============================\n");for(inti=0;i<colorNodes.size();i++){state[i]=colorMap[colorNodes[convertTable[i]].color];printf("color %d: %c %f, %f, %f\n",i,colorNodes[i].color,colorNodes[i].scalar.val[2],colorNodes[i].scalar.val[1],colorNodes[i].scalar.val[0]);}return[[NSStringalloc]initWithCString:stateencoding:NSUTF8StringEncoding];}