/*controle manual via joystick e botoes*/
void manualControl(){
int x, y, digital, b1, b2, end1, end2;
while(1){
x = posicaoJoystick('x');
y = posicaoJoystick('y');
digital = readJSButton();
b1 = readButtons('1');
b2 = readButtons('2');
end1 = readEnd('x');
end2 = readEnd('y');
if(end1||end2){
TM_HD44780_Clear();
TM_HD44780_Puts(0, 0, "FIM DE CURSO");
TM_HD44780_Puts(0, 1, "RESETE A MESA");
return;
}
if(x == 1||x == -1||y == 1||y == -1){
if(x==1){
runClockwise2(1, 'x');
}else if(x==-1){
runCounterclockwise2(1, 'x');
}
if(y==1){
runClockwise2(1, 'y');
}else if(y==-1){
runCounterclockwise2(1, 'y');
}
}
if(digital==1){
TM_HD44780_Clear();
TM_HD44780_Puts(0, 0, "MARCAR");
setFura();
}
if(b1==1){
TM_HD44780_Clear();
TM_HD44780_Puts(0, 0, "DESCER DRILL");
runCaneta(1, '+');
}
if(b2==1){
TM_HD44780_Clear();
TM_HD44780_Puts(0, 0, "SUBIR DRILL");
runCaneta(1, '-');
}
}
}
//------------------------------------------------------------------------------
// send command to card
uint8_t SdReader::cardCommand(uint8_t cmd, uint32_t arg) {
//Serial.print("cardCommand, cmd="); Serial.print(cmd, DEC); Serial.print(", arg="); Serial.println(arg, DEC);
uint8_t r1;
// end read if in partialBlockRead mode
readEnd();
// select card
spiSSLow();
// wait up to 300 ms if busy
waitNotBusy(300);
// send command
spiSend(cmd | 0x40);
// send argument
for (int8_t s = 24; s >= 0; s -= 8) spiSend(arg >> s);
// send CRC
uint8_t crc = 0XFF;
if (cmd == CMD0) crc = 0X95; // correct crc for CMD0 with arg 0
if (cmd == CMD8) crc = 0X87; // correct crc for CMD8 with arg 0X1AA
spiSend(crc);
// wait for response
for (uint8_t retry = 0; ((r1 = spiRec()) & 0X80) && retry != 0XFF; retry++);
return r1;
}
//------------------------------------------------------------------------------
// send command and return error code. Return zero for OK
uint8_t Sd2Card::cardCommand(uint8_t cmd, uint32_t arg) {
// end read if in partialBlockRead mode
readEnd();
// select card
chipSelectLow();
// wait up to 300 ms if busy
waitNotBusy(300);
// send command
spiSend(cmd | 0x40);
// send argument
for (int8_t s = 24; s >= 0; s -= 8) spiSend(arg >> s);
// send CRC
uint8_t crc = 0XFF;
if (cmd == CMD0) crc = 0X95; // correct crc for CMD0 with arg 0
if (cmd == CMD8) crc = 0X87; // correct crc for CMD8 with arg 0X1AA
spiSend(crc);
// wait for response
for (uint8_t i = 0; ((status_ = spiRec()) & 0X80) && i != 0XFF; i++);
return status_;
}
/**
* Read part of a 512 byte block from a SD card.
*
* \param[in] block Logical block to be read.
* \param[in] offset Number of bytes to skip at start of block
* \param[out] dst Pointer to the location that will receive the data.
* \param[in] count Number of bytes to read
* \return The value one, true, is returned for success and
* the value zero, false, is returned for failure.
*/
uint8_t Sd2Card::readData(uint32_t block, uint16_t offset, uint8_t *dst, uint16_t count)
{
if (count == 0) return true;
if ((count + offset) > 512) {
return false;
}
if (!inBlock_ || block != block_ || offset < offset_) {
block_ = block;
//use address if not SDHC card
if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
if (cardCommand(CMD17, block)) {
error(SD_CARD_ERROR_CMD17);
return false;
}
if (!waitStartBlock()) {
return false;
}
offset_ = 0;
inBlock_ = 1;
}
#ifdef OPTIMIZE_HARDWARE_SPI
//start first spi transfer
SPDR = 0XFF;
//skip data before offset
for (;offset_ < offset; offset_++) {
while(!(SPSR & (1 << SPIF)));
SPDR = 0XFF;
}
//transfer data
uint16_t n = count - 1;
for (uint16_t i = 0; i < n; i++) {
while(!(SPSR & (1 << SPIF)));
dst[i] = SPDR;
SPDR = 0XFF;
}
// wait for last byte
while(!(SPSR & (1 << SPIF)));
dst[n] = SPDR;
#else // OPTIMIZE_HARDWARE_SPI
// skip data before offset
for (;offset_ < offset; offset_++) {
spiRec();
}
//transfer data
for (uint16_t i = 0; i < count; i++) {
dst[i] = spiRec();
}
#endif // OPTIMIZE_HARDWARE_SPI
offset_ += count;
if (!partialBlockRead_ || offset_ >= 512) readEnd();
return true;
}
boost::uint32_t StageIterator::read(PointBuffer& buffer)
{
readBegin();
readBufferBegin(buffer);
boost::uint32_t numRead = readBuffer(buffer);
readBufferEnd(buffer);
readEnd();
return numRead;
}
/*
* Read parameter block, including begin and end.
*/
void McSimulation::readParam(std::istream& in)
{
if (isRestarting_) {
if (isInitialized_) {
return;
}
}
readBegin(in, className().c_str());
readParameters(in);
readEnd(in);
}
void Parser::readBlock(Block& block)
{
do {
std::string str = readKeyword();
switch (nextToken()) {
case Parser::Error:
return;
case Parser::Begin:
readBegin();
readBlock(block.addBlock(str));
readEnd();
break;
case Parser::Equal:
readEqual();
if (nextToken() == QuoteStr) {
block.addString(str, readQuotedString());
while (nextToken() == Parser::Comma) {
readComma();
block.addString(str, readQuotedString());
}
} else if (nextToken() == Real) {
block.addReal(str, readReal());
while (nextToken() == Parser::Comma) {
readComma();
block.addReal(str, readReal());
}
} else if (nextToken() == RelativeReal) {
block.addRelativeReal(str, readRelativeReal());
while (nextToken() == Parser::Comma) {
readComma();
block.addRelativeReal(str, readRelativeReal());
}
} else if (nextToken() == Str) {
block.addString(str, readString());
while (nextToken() == Parser::Comma) {
readComma();
block.addString(str, readString());
}
} else {
throw utils::InternalError();
}
readSemicolon();
break;
default:
throw utils::InternalError("bad file");
};
} while (nextToken() != Parser::End and mStream);
}
/*ajusta a posicao zero da mesa xy*/
void calibraZero(){
//move a mesa ate atingir as duas chaves de fim de curso
int i;
TM_HD44780_Clear();
TM_HD44780_Puts(0, 0, "Calibrando...");
while(!(readEnd('x')&&readEnd('y'))){
if(!readEnd('x')){
counterclockwise('x');
}
if(!readEnd('y')){
counterclockwise('y');
}
}
//apos o ajuste de zero, volta um pouco para liberar as chaves
for(i=15; i--;){
clockwise('x');
clockwise('y');
}
TM_HD44780_Clear();
TM_HD44780_Puts(0, 0, "Mesa calibrada");
Delayms(250);
posit_x=0;
posit_y=0;
}
/**
* Read part of a 512 byte block from an SD card.
*
* \param[in] block Logical block to be read.
* \param[in] offset Number of bytes to skip at start of block
* \param[out] dst Pointer to the location that will receive the data.
* \param[in] count Number of bytes to read
* \return The value one, true, is returned for success and
* the value zero, false, is returned for failure.
*/
uint8_t Sd2Card::readData(uint32_t block,
uint16_t offset, uint16_t count, uint8_t* dst) {
uint16_t n;
if (count == 0) return true;
if ((count + offset) > 512) {
goto fail;
}
if (!inBlock_ || block != block_ || offset < offset_) {
block_ = block;
// use address if not SDHC card
if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
if (cardCommand(CMD17, block)) {
error(SD_CARD_ERROR_CMD17);
goto fail;
}
if (!waitStartBlock()) {
goto fail;
}
offset_ = 0;
inBlock_ = 1;
}
#ifdef OPTIMIZE_HARDWARE_SPI
#else // OPTIMIZE_HARDWARE_SPI
// skip data before offset
for (;offset_ < offset; offset_++) {
spiRec();
}
// transfer data
for (uint16_t i = 0; i < count; i++) {
dst[i] = spiRec();
}
#endif // OPTIMIZE_HARDWARE_SPI
offset_ += count;
if (!partialBlockRead_ || offset_ >= 512) {
// read rest of data, checksum and set chip select high
readEnd();
}
return true;
fail:
return false;
}
/**
* Read part of a 512 byte block from a SD card.
*
* \param[in] block Logical block to be read.
* \param[in] offset Number of bytes to skip at start of block
* \param[out] dst Pointer to the location that will receive the data.
* \param[in] count Number of bytes to read
* \return The value one, true, is returned for success and
* the value zero, false, is returned for failure.
*/
uint8_t SdReader::readData(uint32_t block,
uint16_t offset, uint8_t *dst, uint16_t count) {
///Serial.print("readSD, offset="); Serial.print(offset, DEC); Serial.print(", count="); Serial.println(count, DEC);
if (count == 0) return true;
if ((count + offset) > 512) {
return false;
}
if (!inBlock_ || block != block_ || offset < offset_) {
block_ = block;
// use address if not SDHC card
if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
if (cardCommand(CMD17, block)) {
error(SD_CARD_ERROR_CMD17);
return false;
}
if (!waitStartBlock()) {
return false;
}
offset_ = 0;
inBlock_ = 1;
}
// start first SPI transfer
SPDR = 0XFF;
// skip data before offset
for (;offset_ < offset; offset_++) {
while(!(SPSR & (1 << SPIF)));
SPDR = 0XFF;
}
// transfer data
uint16_t n = count - 1;
for (uint16_t i = 0; i < n; i++) {
while(!(SPSR & (1 << SPIF)));
dst[i] = SPDR;
SPDR = 0XFF;
}
// wait for last byte
while(!(SPSR & (1 << SPIF)));
dst[n] = SPDR;
offset_ += count;
if (!partialBlockRead_ || offset_ >= 512) readEnd();
return true;
}
//------------------------------------------------------------------------------
// send command and return error code. Return zero for OK
uint8_t Sd2Card::cardCommand(uint8_t cmd, uint32_t arg) {
// end read if in partialBlockRead mode
readEnd();
// select card
chipSelectLow();
// wait up to 300 ms if busy
waitNotBusy(300);
// send command
spiSend(cmd | 0x40);
#ifdef ESP8266
// send argument
SPI.write32(arg, true);
#else
// send argument
for (int8_t s = 24; s >= 0; s -= 8) spiSend(arg >> s);
#endif
// send CRC
uint8_t crc = 0xFF;
if (cmd == CMD0) crc = 0x95; // correct crc for CMD0 with arg 0
if (cmd == CMD8) crc = 0x87; // correct crc for CMD8 with arg 0X1AA
spiSend(crc);
// wait for response
for (uint8_t i = 0; ((status_ = spiRec()) & 0x80) && i != 0xFF; i++)
;
#ifdef ESP8266
optimistic_yield(10000);
#endif
return status_;
}
/**
* Read part of a 512 byte block from an SD card.
*
* \param[in] block Logical block to be read.
* \param[in] offset Number of bytes to skip at start of block
* \param[out] dst Pointer to the location that will receive the data.
* \param[in] count Number of bytes to read
* \return The value one, true, is returned for success and
* the value zero, false, is returned for failure.
*/
uint8_t Sd2Card::readData(uint32_t block,
uint16_t offset, uint16_t count, uint8_t* dst) {
uint16_t n;
if (count == 0) return true;
if ((count + offset) > 512) {
goto fail;
}
if (!inBlock_ || block != block_ || offset < offset_) {
block_ = block;
// use address if not SDHC card
if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
if (cardCommand(CMD17, block)) {
error(SD_CARD_ERROR_CMD17);
goto fail;
}
if (!waitStartBlock()) {
goto fail;
}
offset_ = 0;
inBlock_ = 1;
}
#ifdef OPTIMIZE_HARDWARE_SPI
// start first spi transfer
SPDR = 0XFF;
// skip data before offset
for (;offset_ < offset; offset_++) {
while (!(SPSR & (1 << SPIF)))
;
SPDR = 0XFF;
}
// transfer data
n = count - 1;
for (uint16_t i = 0; i < n; i++) {
while (!(SPSR & (1 << SPIF)))
;
dst[i] = SPDR;
SPDR = 0XFF;
}
// wait for last byte
while (!(SPSR & (1 << SPIF)))
;
dst[n] = SPDR;
#else // OPTIMIZE_HARDWARE_SPI
SPI.beginTransaction(settings); // *** NEW ***
// skip data before offset
for (;offset_ < offset; offset_++) {
spiRec();
}
// if ((offset-offset_) > 0) { SPI.transfer(offset-offset_, 0xFF); offset_= offset; } // *** NEW ***
// transfer data
//for (uint16_t i = 0; i < count; i++) {
// dst[i] = spiRec();
//}
SPI.transfer(dst, count, 0xFF); // *** NEW ***
#endif // OPTIMIZE_HARDWARE_SPI
offset_ += count;
if (!partialBlockRead_ || offset_ >= 512) {
// read rest of data, checksum and set chip select high
readEnd();
}
#ifdef OPTIMIZE_HARDWARE_SPI
#else // OPTIMIZE_HARDWARE_SPI
SPI.endTransaction(); // *** NEW ***
#endif // OPTIMIZE_HARDWARE_SPI
return true;
fail:
chipSelectHigh();
return false;
}
/**
* Read part of a 512 byte block from an SD card.
*
* \param[in] block Logical block to be read.
* \param[in] offset Number of bytes to skip at start of block
* \param[out] dst Pointer to the location that will receive the data.
* \param[in] count Number of bytes to read
* \return The value one, true, is returned for success and
* the value zero, false, is returned for failure.
*/
uint8_t Sd2Card::readData(uint32_t block,
uint16_t offset, uint16_t count, uint8_t* dst) {
uint16_t n;
if (count == 0) return true;
if ((count + offset) > 512) {
goto fail;
}
if (!inBlock_ || block != block_ || offset < offset_) {
block_ = block;
// use address if not SDHC card
if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
if (cardCommand(CMD17, block)) {
error(SD_CARD_ERROR_CMD17);
goto fail;
}
if (!waitStartBlock()) {
goto fail;
}
offset_ = 0;
inBlock_ = 1;
}
#if defined(USE_TEENSY3_SPI)
// skip data before offset
//for (;offset_ < offset; offset_++) {
//spiRec();
//}
spiRecIgnore(offset);
spiRec(dst, count);
#elif defined(OPTIMIZE_HARDWARE_SPI)
// start first spi transfer
SPDR = 0XFF;
// skip data before offset
for (;offset_ < offset; offset_++) {
while (!(SPSR & (1 << SPIF)));
SPDR = 0XFF;
}
// transfer data
n = count - 1;
for (uint16_t i = 0; i < n; i++) {
while (!(SPSR & (1 << SPIF)));
dst[i] = SPDR;
SPDR = 0XFF;
}
// wait for last byte
while (!(SPSR & (1 << SPIF)));
dst[n] = SPDR;
#else // OPTIMIZE_HARDWARE_SPI
// skip data before offset
for (;offset_ < offset; offset_++) {
spiRec();
}
// transfer data
for (uint16_t i = 0; i < count; i++) {
dst[i] = spiRec();
}
#endif // OPTIMIZE_HARDWARE_SPI
offset_ += count;
if (!partialBlockRead_ || offset_ >= 512) {
// read rest of data, checksum and set chip select high
readEnd();
}
return true;
fail:
chipSelectHigh();
return false;
}
/**
* Read part of a 512 byte block from an SD card.
*
* \param[in] block Logical block to be read.
* \param[in] offset Number of bytes to skip at start of block
* \param[out] dst Pointer to the location that will receive the data.
* \param[in] count Number of bytes to read
* \return The value one, true, is returned for success and
* the value zero, false, is returned for failure.
*/
uint8_t Sd2Card::readData(uint32_t block,
uint16_t offset, uint16_t count, uint8_t* dst) {
//uint16_t n;
if (count == 0) return true;
if ((count + offset) > 512) {
goto fail;
}
if (!inBlock_ || block != block_ || offset < offset_) {
block_ = block;
// use address if not SDHC card
if (type()!= SD_CARD_TYPE_SDHC) block <<= 9;
if (cardCommand(CMD17, block)) {
error(SD_CARD_ERROR_CMD17);
Serial.println("Error: CMD17");
goto fail;
}
if (!waitStartBlock()) {
goto fail;
}
offset_ = 0;
inBlock_ = 1;
}
#ifdef SPI_DMA
// skip data before offset
if(offset_ < offset){
dma_setup_transfer(DMA1,
DMA_CH3,
&SPI1->regs->DR,
DMA_SIZE_8BITS,
ack,
DMA_SIZE_8BITS,
(/*DMA_MINC_MODE | DMA_CIRC_MODE |*/ DMA_FROM_MEM | DMA_TRNS_CMPLT | DMA_TRNS_ERR));
dma_attach_interrupt(DMA1, DMA_CH3, DMAEvent);
dma_set_priority(DMA1, DMA_CH3, DMA_PRIORITY_VERY_HIGH);
dma_set_num_transfers(DMA1, DMA_CH3, offset - offset_);
dmaActive = true;
dma_enable(DMA1, DMA_CH3);
while(dmaActive) delayMicroseconds(1);
dma_disable(DMA1, DMA_CH3);
}
offset_ = offset;
// transfer data
dma_setup_transfer(DMA1, DMA_CH2, &SPI1->regs->DR, DMA_SIZE_8BITS, dst, DMA_SIZE_8BITS,
(DMA_MINC_MODE | DMA_TRNS_CMPLT | DMA_TRNS_ERR));
dma_attach_interrupt(DMA1, DMA_CH2, DMAEvent);
dma_setup_transfer(DMA1, DMA_CH3, &SPI1->regs->DR, DMA_SIZE_8BITS, ack, DMA_SIZE_8BITS,
(/*DMA_MINC_MODE | DMA_CIRC_MODE |*/ DMA_FROM_MEM));
dma_set_priority(DMA1, DMA_CH2, DMA_PRIORITY_VERY_HIGH);
dma_set_priority(DMA1, DMA_CH3, DMA_PRIORITY_VERY_HIGH);
dma_set_num_transfers(DMA1, DMA_CH2, count);
dma_set_num_transfers(DMA1, DMA_CH3, count);
dmaActive = true;
dma_enable(DMA1, DMA_CH3);
dma_enable(DMA1, DMA_CH2);
while(dmaActive) delayMicroseconds(1);
dma_disable(DMA1, DMA_CH3);
dma_disable(DMA1, DMA_CH2);
offset_ += count;
if (!partialBlockRead_ || offset_ >= SPI_BUFF_SIZE) {
readEnd();
}
#else
// skip data before offset
for (;offset_ < offset; offset_++) {
spiRec();
}
// transfer data
for (uint16_t i = 0; i < count; i++) {
dst[i] = spiRec();
}
offset_ += count;
if (!partialBlockRead_ || offset_ >= 512) {
// read rest of data, checksum and set chip select high
readEnd();
}
#endif
return true;
fail:
chipSelectHigh();
Serial.println("Error: Sd2Card::readData()");
return false;
}
void CalaosCameraView::processData()
{
if (!formatDetected)
{
//first frame, we need to look for the boundary
//and for content-type/content-length if present
if (buffer.size() < 4)
return; //need more data
if (buffer[0] != '-' || buffer[1] != '-')
{
//try to detect of the data is directly the jpeg picture
if (buffer[0] == 0xFF && buffer[1] == 0xD8)
{
cWarningDom("camera") << "Data seems to be a single frame.";
single_frame = true;
}
else
{
cWarningDom("camera") << "Wrong start of frame, give up!";
format_error = true;
}
formatDetected = true;
return;
}
//search for the line end after the boundary to get the boundary text
int end, next;
if (!readEnd(2, end, next))
{
if (buffer.size() > 500)
{
cWarningDom("camera") << "Boundary not found, give up!";
format_error = true;
}
return; //need more data;
}
//get boundary
boundary = string((char *)&buffer[0], end);
cDebugDom("camera") << "Found boundary \"" << boundary << "\"";
int i = next;
while (readEnd(next, end, next))
{
int len = end - i;
if (len == 0)
{
//line is empty, data starts now
nextDataStart = next;
formatDetected = true;
scanpos = 0;
break;
}
if (len > 15)
{
string s((char *)&buffer[i], len);
if (Utils::strStartsWith(s, "Content-Length", Utils::CaseInsensitive))
{
Utils::from_string(s.substr(15), nextContentLength);
cDebugDom("camera") << "Found content length header: \"" << nextContentLength << "\"";
//nextContentLength = -1; //to test code without content-length header
}
}
i = next;
}
if (!formatDetected)
{
cWarningDom("camera") << "need more data...";
return;
}
}
if (formatDetected && !single_frame)
{
//we should be positionned at the start of data
//small check to be sure
if (buffer.size() <= nextDataStart)
{
cWarningDom("camera") << "need more data...";
return;
}
if (!(buffer[nextDataStart] == 0xFF && buffer[nextDataStart + 1] == 0xD8))
{
cWarningDom("camera") << "Wrong image data.";
format_error = true;
EcoreTimer::singleShot(0, [=]()
{
cDebugDom("camera") << "Cancel stream";
ecore_con_url_free(ecurl);
ecurl = nullptr;
});
return;
}
if (nextContentLength >= 0)
{
//the content-length is known, fast path
if ((int)buffer.size() < nextContentLength + nextDataStart + 2)
return; //need more data
cDebugDom("camera") << "Set new frame";
evas_object_image_memfile_set(camImage, &buffer[nextDataStart], nextContentLength, NULL, NULL);
//evas_object_image_size_get(camImage, &w, &h);
if (buffer[nextDataStart + nextContentLength] == '\r') //assume a \n always follows \r
nextContentLength += 2;
else if (buffer[nextDataStart + nextContentLength] == '\n')
nextContentLength += 1;
//remove unused data from buffer
auto iter = buffer.begin();
buffer.erase(iter, iter + (nextDataStart + nextContentLength));
//reset for next frame
nextContentLength = -1;
formatDetected = false;
nextDataStart = 0;
scanpos = 0;
}
else
{
uint i = 0;
cDebugDom("camera") << "scanpos: " << scanpos;
for (i = nextDataStart + scanpos;
i < buffer.size() - boundary.length();i++)
{
if (buffer[i] == '-' && buffer[i + 1] == '-' &&
!boundary.compare(0, boundary.length(), (const char *)&buffer[i], boundary.length()))
{
//boundary found
//check for newline between boundary and data
nextContentLength = i - nextDataStart;
if (buffer[i - 2] == '\r')
nextContentLength -= 2;
else if (buffer[i - 1] == '\n')
nextContentLength -= 1;
evas_object_image_memfile_set(camImage, &buffer[nextDataStart], nextContentLength, NULL, NULL);
//evas_object_image_size_get(camImage, &w, &h);
if (buffer[nextDataStart + nextContentLength] == '\r') //assume a \n always follows \r
nextContentLength += 2;
else if (buffer[nextDataStart + nextContentLength] == '\n')
nextContentLength += 1;
//remove unused data from buffer
auto iter = buffer.begin();
buffer.erase(iter, iter + (nextDataStart + nextContentLength));
//reset for next frame
nextContentLength = -1;
formatDetected = false;
nextDataStart = 0;
scanpos = 0;
cDebugDom("camera") << "scanpos: " << scanpos << " --> return";
return;
}
}
scanpos = i - nextDataStart;
cDebugDom("camera") << "--> scanpos: " << scanpos;
}
}
}
/**
Enable or disable partial block reads.
Enabling partial block reads improves performance by allowing a block
to be read over the SPI bus as several sub-blocks. Errors may occur
if the time between reads is too long since the SD card may timeout.
The SPI SS line will be held low until the entire block is read or
readEnd() is called.
Use this for applications like the Adafruit Wave Shield.
\param[in] value The value TRUE (non-zero) or FALSE (zero).)
*/
void Sd2Card::partialBlockRead(uint8_t value)
{
readEnd();
partialBlockRead_ = value;
}
Istream& Istream::readEndBegin(const char* funcName)
{
readEnd(funcName);
return readBegin(funcName);
}
