long long moveTest(Board_t self, int depth)
{
if (depth <= 0)
return 1;
long long total = 0;
int moveList[maxMoves];
int nrMoves = generateMoves(self, moveList);
for (int i=0; i<nrMoves; i++) {
makeMove(self, moveList[i]);
if (wasLegalMove(self))
total += moveTest(self, depth - 1);
undoMove(self);
}
return total;
}
int main(void){
int log = 0;
//Start Switch
// DDRA = 0x00;
// PORTA = 0x12;
//Start PORT A for switch and IR sensors
DDRA = 0xFC;
PORTA = 0xFE;
//LED Initial
DDRC = 0x7F;
PORTC = 0x7E;
DDRD = 0x70;
PORTD = 0x11;
MotorInit();
initSerial();
char * readData = NULL;
int isFinish = 0;
sensorInit();
if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
while(1){
sensorTest(0);
sensorTest(1);
sensorTest(2);
setMode();
if( checkSerialRead() > 0 ){
readData = getReadBuffer();
if( readData != NULL ){
// printf( "readData=%s\n", &readData[0] );
split( &readData[0] );
switch( serCmd[0] ){
case EVT_ACTION:
ServoControl( serCmd[1] );
// setSpeedTest( serCmd[1] );
sendAck(1);
break;
case EVT_START_MOTION:
startMotion( serCmd[1], serCmd[2] );
PORTC = ~(1 << (LED_MAX - 2));
sendAck(1);
break;
case EVT_STOP_MOTION:
stopMotion();
sendAck(1);
break;
case EVT_FORCE_MOTION:
forceMotion( serCmd[1], serCmd[2] );
break;
case EVT_GET_NOW_ANGLE:
getAngle();
break;
case EVT_SET_ANGLE:
setAngle();
case EVT_GET_ACT_ANGLE:
if( serCmd[1] >= ACT_MAX ){
sendAck(0);
}else{
sendActAngle(serCmd[1]);
}
break;
case EVT_GET_LOAD:
getLoad();
// printf( "%d\n", movingTime );
break;
case EVT_GET_VOLTAGE:
getVoltage();
break;
case EVT_TORQUE_DISABLE:
dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
break;
case EVT_WATCH_DOG:
watchDogCnt = 0;
break;
case EVT_MOTION_EDIT:
break;
case 999:
// printf( "finish\n");
sendAck(999);
isFinish = 1;
break;
default:
sendAck(0);
}
if( isFinish > 0 ){
MotorControl( 0, 0 );
break;
}
memset( readData, 0x00, SERIAL_BUFFER_SIZE );
}
}
memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE );
if (~PINA & SW_START ) {
if (log == 1) printf( "main() 0\n");
if( iStart > 0 ){
iStart = 0;
PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY;
if (isCaptureMode != 1) ServoControl( 0 );
}
}else{
if( iStart == 0 ){
PORTC &= ~LED_PLAY;
iStart = 1;
}
if( modeWait <= 0 ){
if (log == 1) printf( "main() 1\n");
setModeAction();
if (isMovetest == 1) {
moveTest();
} else {
move();
}
}else{
if (log == 1) printf( "main() 2\n");
modeWait -= MAIN_DELAY;
}
}
if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) {
if (log == 1) printf( "### main() sensorValue[0] == 0\n");
PORTC |= LED_PROGRAM; //edit
}else if (sensorValueOld[0] != sensorValue[0]){
if (log == 1) printf( "### main() sensorValue[0] == 1\n");
PORTC &= ~LED_PROGRAM; //edit
}
if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) {
if (log == 1) printf( "### main() sensorValue[1] == 0\n");
PORTC |= LED_MANAGE; //mon
}else if (sensorValueOld[1] != sensorValue[1]){
if (log == 1) printf( "### main() sensorValue[1] == 1\n");
PORTC &= ~LED_MANAGE; //mon
}
if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) {
if (log == 1) printf( "### main() sensorValue[2] == 0\n");
PORTC |= LED_AUX; //AUX
}else if (sensorValueOld[2] != sensorValue[2]){
if (log == 1) printf( "### main() sensorValue[2] == 1\n");
PORTC &= ~LED_AUX; //AUX
}
sensorValueOld[0] = sensorValue[0];
sensorValueOld[1] = sensorValue[1];
sensorValueOld[2] = sensorValue[2];
// walk pattern LED
// brinkLED();
_delay_ms(MAIN_DELAY);
watchDogCnt++;
caputureCount1++;
if (caputureCount1 == 25){
if (isCaptureMode == 1) getAngle();
caputureCount1 = 0;
}
}
}
