void run()
{
uint8_t key[2];
uint8_t key_itr = 0;
while(key_itr < 2)
key[++key_itr] = 0;
key_itr = 0;
moveflags = 0;
recordIter = 0;
speed = 127;
le.clear();
re.clear();
recordTime.stop();
recordTime.clear();
uint32_t nextPlay = 0;
uint32_t nextPlayBase = 0;
state = 0;
encoder_play_l.stop();
encoder_play_r.stop();
startTime = 0;
/*rs232.send("YuniRC program has started!\r\n"
"Controls: W,A,S,D - movement, Space - read sensor values,");
rs232.wait();
rs232.send(" R - reset encoders, Q - On/Off engine correction, 1 2 3 - speed \r\n");
rs232.wait();
rs232.send("Engine correction is disabled.\r\n"); */
char ch;
while(true)
{
if(state & STATE_ERASING)
continue;
// Move correction
if((state & STATE_CORRECTION) && (moveflags == MOVE_FORWARD || moveflags == MOVE_BACKWARD))
MovementCorrection();
if((state & STATE_PLAY) && (lastAdress == 0 || EventHappened(&lastRec, &nextPlayBase, &nextPlay)))
{
encoder_play_l.stop();
encoder_play_r.stop();
read_mem(&lastRec, lastAdress);
lastAdress += REC_SIZE;
if((lastRec.key[0] == 0 && lastRec.key[1] == 0 && lastRec.getBigNum() == 0) ||
lastAdress > 512)
{
state &= ~(STATE_PLAY);
rs232.send("Playback finished\r\n");
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
continue;
}
SetMovement(lastRec.key);
nextPlay = 0;
nextPlayBase = 0;
if(lastRec.end_event == EVENT_TIME)
{
nextPlayBase = getTickCount();
nextPlay = (uint32_t(lastRec.getBigNum())*10000) * JUNIOR_WAIT_MUL / JUNIOR_WAIT_DIV;
}
//Uncomment to set messure delay
/*else if(lastRec.end_event == EVENT_RANGE_MIDDLE_HIGHER || lastRec.end_event == EVENT_RANGE_MIDDLE_LOWER)
{
nextPlayBase = getTickCount();
nextPlay = (50000) * JUNIOR_WAIT_MUL / JUNIOR_WAIT_DIV;
}*/
else if(lastRec.end_event == EVENT_DISTANCE || lastRec.end_event == EVENT_DISTANCE_LEFT || lastRec.end_event == EVENT_DISTANCE_RIGHT)
{
encoder_play_r.clear();
encoder_play_l.clear();
encoder_play_l.start();
encoder_play_r.start();
}
++recordIter;
}
//Read command
if(!rs232.peek(ch))
continue;
key[key_itr] = uint8_t(ch);
++key_itr;
//key recieved
if(key_itr >= 2)
{
key_itr = 0;
// FIXME: ignore two or more keys at once
if((state & STATE_RECORD) && char(lastRec.key[1]) == 'd' && char(key[1]) != 'u' && char(key[0]) != 'C')
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
continue;
}
bool down_only = SetMovement(key);
if(char(key[0]) == 'O' || char(key[0]) == 'P')
continue;
else if((state & STATE_RECORD) && char(key[0]) != 'C' &&
(!down_only || (down_only && char(key[1]) == 'd'))) // do not record down only keys
{
if(!recordTime.isRunning())
{
recordTime.clear();
recordTime.start();
}
if(recordIter > 0)
{
lastRec.end_event = EVENT_TIME;
lastRec.setBigNum(recordTime.getTime()/10000);
write_mem(&lastRec, lastAdress);
lastAdress+=REC_SIZE;
}
if(recordIter < MEM_SIZE-1)
{
while(key_itr < 2)
{
lastRec.key[key_itr] = key[key_itr];
++key_itr;
}
key_itr = 0;
recordTime.clear();
++recordIter;
}
else
{
key[0] = uint8_t('C');
key[1] = uint8_t('d');
rs232.send("Memory full\r\n");
continue;
}
}
}
// EEPROM Flash mode
else if(ch == 0x1C)
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
erase_eeprom();
rs232.sendCharacter(0x1D);
for(lastAdress = 0; true; )
{
if(!rs232.peek(ch))
continue;
if(ch == 0x1E && lastAdress%5 == 0)
break;
write_byte(lastAdress, uint8_t(ch));
++lastAdress;
rs232.sendCharacter(0x1F);
}
lastAdress = 0;
}
// EEPROM read mode
else if(ch == 0x16)
{
while(key_itr < 2)
key[++key_itr] = '0';
key_itr = 0;
rs232.sendCharacter(0x17);
for(lastAdress = 0; lastAdress < 512; ++lastAdress)
{
rs232.wait();
rs232.sendCharacter(read_byte(lastAdress));
}
rs232.sendCharacter(0x18);
lastAdress = 0;
}
}
}
