static void
rangeaccess(void *addr, uintptr size, uintptr step, uintptr callpc, uintptr pc, bool write)
{
uintptr racectx;
if(!onstack((uintptr)addr)) {
m->racecall = true;
racectx = g->racectx;
if(callpc) {
if(callpc == (uintptr)runtime·lessstack)
runtime·callers(3, &callpc, 1);
runtime∕race·FuncEnter(racectx, (void*)callpc);
}
if(write)
runtime∕race·WriteRange(racectx, addr, size, step, (void*)pc);
else
runtime∕race·ReadRange(racectx, addr, size, step, (void*)pc);
if(callpc)
runtime∕race·FuncExit(racectx);
m->racecall = false;
}
}
bool SetMovement(uint8_t key[])
{
// Set Movement Flags
bool down = (key[1] == uint8_t('d'));
bool down_only = false;
// only down keys
if(down)
{
switch(char(key[0]))
{
//speed (1 2 3 on keyboard Oo)
case 'a': speed = 50; break;
case 'b': speed = 100; break;
case 'c': speed = 127; break;
case 'R': // reset encoders
re.clear();
le.clear();
break;
case 'Q': // on/off correction
rs232.send("Engine correction is ");
if(state & STATE_CORRECTION)
{
state &= ~(STATE_CORRECTION);
rs232.send("disabled \r\n");
}
else
{
state |= STATE_CORRECTION;
rs232.send("enabled \r\n");
}
break;
case 'C':
rs232.wait();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
if(!(state & STATE_RECORD))
{
state |= STATE_RECORD;
recordIter = 0;
rs232.send("Erasing EEPROM...");
state |= STATE_ERASING;
erase_eeprom();
state &= ~(STATE_ERASING);
rs232.send("done\r\n");
lastAdress = 0;
}
else
{
lastRec.end_event = EVENT_TIME;
lastRec.setBigNum(recordTime.getTime()/10000);
write_mem(&lastRec, lastAdress);
recordTime.stop();
recordTime.clear();
recordIter = 0;
state &= ~(STATE_RECORD);
}
rs232.send("Trace recording is ");
if(state & STATE_RECORD){ rs232.send("enabled \r\n");}
else {rs232.send("disabled \r\n");}
break;
case 'P':
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
setMotorPower(0, 0);
if(!(state & STATE_PLAY))
{
recordTime.stop();
recordTime.clear();
rs232.send("Playing..\r\n");
recordIter = 0;
lastAdress = 0;
state |= STATE_PLAY;
state &= ~(STATE_RECORD);
}
else
{
rs232.send("Playback stopped\r\n");
state &= ~(STATE_PLAY);
}
break;
case 'O':
if(state & STATE_RECORD)
break;
rs232.send("Playback ");
if(state & STATE_PLAY)
{
rs232.send("unpaused\r\n");
state &= ~(STATE_PLAY);
}
else
{
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
rs232.send("paused\r\n");
state |= STATE_PLAY;
}
break;
}
}
// Movement
switch(char(key[0]))
{
case 'W':
if(!(moveflags & MOVE_BACKWARD))
{
if(down) moveflags |= MOVE_FORWARD;
else moveflags &= ~(MOVE_FORWARD);
}
break;
case 'S':
if(!(moveflags & MOVE_FORWARD))
{
if(down) moveflags |= MOVE_BACKWARD;
else moveflags &= ~(MOVE_BACKWARD);
}
break;
case 'A':
if(!(moveflags & MOVE_RIGHT))
{
if(down) moveflags |= MOVE_LEFT;
else moveflags &= ~(MOVE_LEFT);
}
break;
case 'D':
if(!(moveflags & MOVE_LEFT))
{
if(down) moveflags |= MOVE_RIGHT;
else moveflags &= ~(MOVE_RIGHT);
}
break;
default:
down_only = true;
break;
}
// Sensors
if(char(key[0]) == ' ' && down) // Space
{
rs232.wait();
rs232.send("\r\nSensors: ");
rs232.dumpNumber(getSensorValue(6));
rs232.sendNumber(getSensorValue(7)); // proud
/*rs232.send("\r\nSensors: \r\n");
rs232.send(" ");
rs232.sendNumber(getSensorValue(5));
rs232.send(" ");
rs232.sendNumber(getSensorValue(1));
rs232.wait();
rs232.send("\r\n");
rs232.sendNumber(getSensorValue(2));
rs232.send(" ");
rs232.sendNumber(getSensorValue(3));
rs232.wait(); */
rs232.send("\r\nEncoders: \r\n L: ");
rs232.sendNumber(le.get());
rs232.send(" R: ");
rs232.sendNumber(re.get());
rs232.send("\r\nRange \r\nL: ");
rs232.wait();
rs232.sendNumber(ReadRange(FINDER_LEFT));
rs232.send("cm M: ");
rs232.sendNumber(ReadRange(FINDER_MIDDLE));
rs232.send("cm R: ");
rs232.sendNumber(ReadRange(FINDER_RIGHT));
rs232.send("cm\r\n");
}
//Set motors
if(moveflags & MOVE_FORWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(speed-TURN_VALUE, speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(speed, speed-TURN_VALUE);
else
{
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(speed, speed);
state &= ~(STATE_CORRECTION2);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_BACKWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(-(speed-TURN_VALUE), -speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(-speed, -(speed-TURN_VALUE));
else
{
state &= ~(STATE_CORRECTION2);
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(-speed, -speed);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_LEFT)
{
setMotorPower(-speed, speed);
startTime = getTickCount();
}
else if(moveflags & MOVE_RIGHT)
{
setMotorPower(speed, -speed);
startTime = getTickCount();
}
else
{
startTime = getTickCount();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
state &= ~(STATE_CORRECTION2);
}
return down_only;
}