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; } } }