void MainWindow::GameThread::run()
{
clearInterrupt();
setTerminationEnabled(true);
for (; undoSteps > 0; --undoSteps)
g->undo();
while (!g->isFinished())
{
msleep(PLAYER_GAP_TIMEOUT);
g->step();
if (isInterrupted() && undoSteps == 0)
break;
else
clearInterrupt();
for (; undoSteps > 0; --undoSteps)
{
while (g->undo())
{}
}
}
}
void ADCSequencer::triggerAndWait()
{
trigger();
while (!isDataReady()) { Task::yield(); }
readData();
clearInterrupt();
}
Timer* requestCurrentTimer(u64 timeout, void (*onTimer)())
{
clearInterrupt();
timeout+=timeManager.time;
Timer* timer = requestTimer(timeout, onTimer);
setupInterrupt();
}
/***********************************************************
*
* update
*
*
*
***********************************************************/
void MMA8452_ahref::update()
{
if(dataMode_)
{
xyz(x_,y_,z_);
}
if(shakeMode_ == true || motionMode_ == true)
{
clearInterrupt();
}
}
Timer* requestTimer(u64 timeout, void (*onTimer)())
{
clearInterrupt();
int i=0, j=0;
Timer *timer = null;
for (i=0;i<MAX_TIMER_NUM;++i) {
if (timeManager.timerList[i].status == STATUS_TIMER_UNUSE) {
timeManager.timerList[i].status = STATUS_TIMER_USING;
timeManager.timerList[i].timeout = timeout;
timeManager.timerList[i].onTimer = onTimer;
timeManager.timerList[i].next = null;
if (timeManager.latest>timeout) {
timeManager.latest=timeout;
}
timer = &timeManager.timerList[i];
break;
}
}
if (timer != null) {
if (timeManager.currentTimer==null) {
timeManager.currentTimer = timer;
} else {
Timer *prevTimer = timeManager.currentTimer, *nextTimer = null;
if ((*prevTimer).timeout>(*timer).timeout) {
(*timer).next = prevTimer;
timeManager.currentTimer = timer;
} else {
while(TRUE) {
nextTimer = (*prevTimer).next;
if (nextTimer != null) {
if ((*nextTimer).timeout>(*timer).timeout) {
(*prevTimer).next = timer;
(*timer).next = nextTimer;
break;
} else {
prevTimer = nextTimer;
}
} else {
(*prevTimer).next = timer;
break;
}
}
}
setupInterrupt();
return timer;
}
}
setupInterrupt();
return null;
}
void initSystem(void)
{
mouseData.phase = 0;
initVideoModeInfo();
initInterruptHandler();
initQueueBufferData();
initTimerManagement();
initPeripheralStatus();
initKeyTableSetting();
initMemoryManagement();
initSheetManagement();
initProcessManagement();
prepareKernelProcess();
initDesktopInfoSheet();
testSheet = prepareSheet();
prepareTestSheet(testSheet);
loadWindowSheet(testSheet);
while(TRUE) {
clearInterrupt();
if (queueBufferStatus(&systemBuffer) == 0) {
setupInterrupt();
} else {
u32 data = getQueueBuffer(&systemBuffer);
if (data<1024) {
setupInterrupt();
} else if (data>=1024 && data<2048) {
Sheet *commandWindow = (*commandProcess).mainWindow;
processKeyData(&keyData, data, commandWindow);
} else if (data>=2048) {
processMouseData(&mouseData, data, mouse, &mx, &my);
}
}
}
}
bool DWM1000_Anchor::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
while (true) {
WAIT_POLL: {
dwt_setrxtimeout(0); /* Clear reception timeout to start next ranging process. */
dwt_rxenable(0); /* Activate reception immediately. */
// dwt_setinterrupt(DWT_INT_RFCG, 1); // enable RXD interrupt
while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(1000);/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
clearInterrupt();
PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = _status_reg;
LOG<< HEX << " status reg.:" << status_reg << " ,interrupts : " << interruptCount << FLUSH;
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
LOG<< HEX << " IRQ pin : " << digitalRead(D2) << " status_reg DWM1000 " << status_reg << FLUSH;// PULL LOW
if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
break;
}
}
///____________________________________________________________________________
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $ "<<FLUSH;
uint32 frame_len;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); /* Clear good RX frame event in the DW1000 status register. */
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is a poll sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) {
LOG<< " $$ "<<FLUSH;
uint32 resp_tx_time;
poll_rx_ts = get_rx_timestamp_u64(); /* Retrieve poll reception timestamp. */
/* Set send time for response. See NOTE 8 below. */
resp_tx_time = (poll_rx_ts
+ (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(resp_tx_time);
/* Set expected delay and timeout for final message reception. */
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
/* Write and send the response message. See NOTE 9 below.*/
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
* See NOTE 7 below. */
// while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(10);
dwt_setinterrupt(DWT_INT_RFCG, 1);// enable
clearInterrupt();
// PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
// status_reg = _status_reg;
LOG<< HEX << " status reg2:" << status_reg << FLUSH;
// if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
// break;
// }
// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
// { };
/* Increment frame sequence number after transmission of the response message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $$$ "<<FLUSH;
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID,
SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(
RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is a final message sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN)
== 0) {
uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
double Ra, Rb, Da, Db;
int64 tof_dtu;
/* Retrieve response transmission and final reception timestamps. */
resp_tx_ts = get_tx_timestamp_u64();
final_rx_ts = get_rx_timestamp_u64();
/* Get timestamps embedded in the final message. */
final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX],
&poll_tx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX],
&resp_rx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX],
&final_tx_ts);
/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
poll_rx_ts_32 = (uint32) poll_rx_ts;
resp_tx_ts_32 = (uint32) resp_tx_ts;
final_rx_ts_32 = (uint32) final_rx_ts;
Ra = (double) (resp_rx_ts - poll_tx_ts);
Rb = (double) (final_rx_ts_32 - resp_tx_ts_32);
Da = (double) (final_tx_ts - resp_rx_ts);
Db = (double) (resp_tx_ts_32 - poll_rx_ts_32);
tof_dtu = (int64) ((Ra * Rb - Da * Db)
/ (Ra + Rb + Da + Db));
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
/* Display computed distance on LCD. */
// char dist_str[20];
// sprintf(dist_str,"%3.2f", distance);
// lcd_display_str(dist_str);
LOG<< " distance : " << (float)distance << "m. " << FLUSH;
}
} else {
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
} else {
void GroveColorSensor::readRGB()
{
Wire.beginTransmission(sensorAddress_);
Wire.write(REG_BLOCK_READ);
Wire.endTransmission();
Wire.beginTransmission(sensorAddress_);
Wire.requestFrom(sensorAddress_, 8);
delay(100);
// if two bytes were received
if(8 <= Wire.available())
{
int i;
for(i = 0; i < 8; ++i)
{
readingdata_[i] = Wire.read();
//Serial.println(readingdata_[i], BIN);
}
}
green_ = readingdata_[1] * 256 + readingdata_[0];
red_ = readingdata_[3] * 256 + readingdata_[2];
blue_ = readingdata_[5] * 256 + readingdata_[4];
clear_ = readingdata_[7] * 256 + readingdata_[6];
Serial.print("The RGBC value are: RGBC( ");
Serial.print(red_,DEC);
Serial.print(", ");
Serial.print(green_,DEC);
Serial.print(", ");
Serial.print(blue_,DEC);
Serial.print(", ");
Serial.println(" )");
Serial.print("The Clear channel value are: ");
Serial.println(clear_,DEC);
Serial.println(" )");
red_ = red_ - R0_;
green_ = green_ - G0_;
blue_ = blue_ - B0_;
/* red_ = red_ - 10;
green_ = green_ - 20;
blue_ = blue_ - 20; */
double tmp;
int maxColor;
// red_ = red_ * 1.62; //白平衡调整
// blue_ = blue_ * 1.017;
red_ = red_ * kr_; //白平衡调整
blue_ = blue_ * kb_;
maxColor = max(red_, green_);
maxColor = max(maxColor, blue_);
if(maxColor > 255)
{
tmp = 250.0/maxColor;
green_ *= tmp;
red_ *= tmp;
blue_ *= tmp;
}
int minColor = min(red_, green_);
minColor = min(minColor, blue_);
maxColor = max(red_, green_);
maxColor = max(maxColor, blue_);
int greenTmp = green_;
int redTmp = red_;
int blueTmp = blue_;
// Serial.print("The RGB value are: RGB( ");
// Serial.print(red_,DEC);
// Serial.print(", ");
// Serial.print(green_,DEC);
// Serial.print(", ");
// Serial.print(blue_,DEC);
// Serial.println(" )");
// Serial.print("The Clear channel value are: ");
// Serial.println(clear_,DEC);
// when turn on LED, need to adjust the RGB data,otherwise it is almost the white color
//以下做一下滤波处理
if(red_ < 0.8*maxColor && red_ >= 0.6*maxColor)
{
red_ *= 0.4;
}
else if(red_ < 0.6*maxColor)
{
red_ *= 0.2;
}
if(green_ < 0.8*maxColor && green_ >= 0.6*maxColor)
{
green_ *= 0.4;
}
else if(green_ < 0.6*maxColor)
{
if (maxColor == redTmp && greenTmp >= 2*blueTmp && greenTmp >= 0.2*redTmp) //orange
{
green_ *= 5;
}
green_ *= 0.2;
}
if(blue_ < 0.8*maxColor && blue_ >= 0.6*maxColor && blue_ > 125)
{
blue_ *= 0.4;
}
else if(blue_ < 0.6*maxColor)
{
if (maxColor == redTmp && greenTmp >= 2*blueTmp && greenTmp >= 0.2*redTmp) //orange
{
blue_ *= 0.5;
}
if (maxColor == redTmp && greenTmp <= blueTmp && blueTmp >= 0.2*redTmp) //pink
{
blue_ *= 5;
}
blue_ *= 0.2;
}
minColor = min(red_, green_);
minColor = min(minColor, blue_);
if(maxColor == green_ && red_ >= 0.85*maxColor && minColor == blue_) //yellow
{
red_ = maxColor;
blue_ *= 0.4;
}
// Serial.print("The RGB value are: RGB( ");
// Serial.print(red_,DEC);
// Serial.print(", ");
// Serial.print(green_,DEC);
// Serial.print(", ");
// Serial.print(blue_,DEC);
// Serial.println(" )");
// Serial.print("The Clear channel value are: ");
// Serial.println(clear_,DEC);
clearInterrupt();
}
bool DWM1000_Tag::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
POLL_SEND: {
while (true) {
timeout(1000); // delay between POLL
PT_YIELD_UNTIL(timeout());
/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
dwt_writetxfctrl(sizeof(tx_poll_msg), 0);
/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
* set by dwt_setrxaftertxdelay() has elapsed. */
LOG<< " Start TXF " << FLUSH;
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);// SEND POLL MSG
dwt_setinterrupt(DWT_INT_TFRS, 0);
dwt_setinterrupt(DWT_INT_RFCG, 1); // enable
clearInterrupt();
_timeoutCounter = 0;
/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
timeout(10);
PT_YIELD_UNTIL(timeout() || isInterruptDetected()); // WAIT RESP MSG
if (isInterruptDetected())
LOG<< " INTERRUPT DETECTED " << FLUSH;
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
LOG<< HEX <<" SYS_STATUS " << status_reg << FLUSH;
if (status_reg == 0xDEADDEAD) {
init();
} else if (status_reg & SYS_STATUS_RXFCG)
goto RESP_RECEIVED;
else if (status_reg & SYS_STATUS_ALL_RX_ERR) {
if (status_reg & SYS_STATUS_RXRFTO)
INFO(" RX Timeout");
else
INFO(" RX error ");
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); /* Clear RX error events in the DW1000 status register. */
}
}
}
RESP_RECEIVED: {
LOG<< " Received " <<FLUSH;
frame_seq_nb++; /* Increment frame sequence number after transmission of the poll message (modulo 256). */
uint32 frame_len;
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID,
SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read iCHANGEt into the local buffer. */
frame_len =
dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is the expected response from the companion "DS TWR responder" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0) { // CHECK RESP MSG
uint32 final_tx_time;
/* Retrieve poll transmission and response reception timestamp. */
poll_tx_ts = get_tx_timestamp_u64();
resp_rx_ts = get_rx_timestamp_u64();
/* Compute final message transmission time. See NOTE 9 below. */
final_tx_time = (resp_rx_ts
+ (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME))
>> 8;
dwt_setdelayedtrxtime(final_tx_time);
/* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
final_tx_ts = (((uint64) (final_tx_time & 0xFFFFFFFE)) << 8)
+ TX_ANT_DLY;
/* Write all timestamps in the final message. See NOTE 10 below. */
final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX],
poll_tx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX],
resp_rx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX],
final_tx_ts);
/* Write and send final message. See NOTE 7 below. */
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
dwt_writetxfctrl(sizeof(tx_final_msg), 0);
dwt_starttx(DWT_START_TX_DELAYED); // SEND FINAL MSG
/* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
timeout(10);
PT_YIELD_UNTIL((dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS) || timeout());;
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the final message (modulo 256). */
frame_seq_nb++;
} else {
