static bool pwm_running() { //TODO Rename return (millis() - myPwmBldcMotor.pwm.motorConfigTime >= ESC_START_DELAY); }
Serial.print("microphone... "); lightwalker.initAudio(); Serial.println("check"); // <cgerstle> Join i2c bus as master Serial.print("i2c bus..."); Wire.begin(); Serial.println("check"); Serial.println("legs..."); lightwalker.initLegs(masterOff); Serial.println(" check"); Serial.println("Walking!"); } unsigned long statusTimer = millis(); unsigned long currentTime = millis(); int frameCounter = 0; void loop() { random16_add_entropy(analogRead(FLOATING_PIN)); currentTime = millis(); frameCounter++; if (currentTime >= (statusTimer + 1000)) { statusTimer = currentTime; Serial.print("["); Serial.print(frameCounter); Serial.println("]"); frameCounter = 0; }
int main() { // SETUP init(); Serial.begin(9600); tft.initR(INITR_BLACKTAB); // initialize screen // Setting the joystick button and LEDs pinMode(JOYSTICK_BUTTON, INPUT); digitalWrite(JOYSTICK_BUTTON, HIGH); // Initialize the SD card Serial.print("Initializing SD card..."); if (!SD.begin(SD_CS)) { Serial.println("failed!"); while(true) {} // something is wrong } else {Serial.println("OK!");} // More initialization Serial.print("Initializing Raw SD card..."); if (!card.init(SPI_HALF_SPEED, SD_CS)) { Serial.println("failed!"); while(true) {} // something is wrong } else {Serial.println("OK!");} // Create states for different modes // C1 for Mode 1 - MENU screen // C2 for Mode 2 - Snake Game // C3 for Mode 3 - GAME OVER screen // C4 for Mode 4 - Choose level // C5 for Mode 5 - PAUSE screen typedef enum {C1 = 1, C2, C3, C4, C5, ERR} State; State state = C1; int select, snakelength; while (state!=ERR) { if (state == C1) { /// ====== MODE 1 - MENU ====== /// Serial.println("Currently in Mode 1"); snakelength = 1; init_vert = analogRead(JOYSTICK_VERT); init_horiz = analogRead(JOYSTICK_HORIZ); // preparations for the game - to not overlap with the pause menu q = q_create(720); i = 64; // x component j = 80; // y component q_add(q,i,j); // load into the queue random_x = food_x(); // load x coordinate of food piece random_y = food_y(); // load y coordinate of food piece pausedirection = 0; // set paused direction to 0 // reset grid to 0 for (int a = 0; a < 24; a++) { for (int b = 0; b < 30; b++) { grid[a][b] = 0; } } // display main menu snake(); tft.setTextSize(2); while(true) { // alternate highlighting of START unsigned long time = millis()%1000; int a = time%1000; if ((a<17)) { tft.setCursor(34, 83); tft.fillRoundRect(30,80,65,20,5,WHITE); tft.setTextColor(RED); tft.print("START"); } else if ((a>500) && (a<520)) { tft.setCursor(34, 83); tft.fillRoundRect(30,80,65,20,5,RED); tft.setTextColor(WHITE); tft.print("START"); } // Read the Joystick - HIGH if not pressed, LOW otherwise select = digitalRead(JOYSTICK_BUTTON); if (select == LOW) { break; } } state = C4; } else if (state == C2) { /// ====== MODE 2 - SNAKE GAME ====== /// Serial.println("Currently in Mode 2"); delay(50); soundsetup(); //setting up sound pin // print the background tft.fillScreen(DARKRED); tft.fillRect(4,5,120,150,DARKGRN); // print the snake int x,y; x = q_frontx(q); y = q_fronty(q); tft.fillRect(x,y,5,5, WHITE); //Bringing the food in, outside while loop first. tft.fillRect(random_x, random_y, 5, 5, YELLOW); // do auto calibration int px, py; int lastmove; // read beginning direction chosen by user if (pausedirection == 0) { direction = read_direction(); } else { direction = pausedirection; } lastmove = direction; while (true) { // to direct movement // (without going in reverse direction of previous movement) // up if (direction == 1) { if (lastmove == 2) { direction = 2; j = j-5; } else { j = j+5; } q_add(q,i,j); } // down else if (direction == 2) { if (lastmove == 1) { direction = 1; j = j+5; } else { j = j-5; } q_add(q,i,j); } // right else if (direction == 3) { if (lastmove == 4) { direction = 4; i = i-5; } else { i = i+5; } q_add(q,i,j); } // left else if (direction == 4) { if (lastmove == 3) { direction = 3; i = i+5; } else { i = i-5; } q_add(q,i,j); } // if the direction is changed, store the new direction & last move int new_direc = read_direction(); if ((new_direc != direction) && (new_direc != 0)) { lastmove = direction; direction = new_direc; } // if the snake hits a piece of food, the food vanishes and gets replaced if ((i == random_x) && (j == random_y)) { // snake grows by 4 squares, except for the first time // this allows for it to end up as a max of 720 in the queue if (snakelength == 1) { q_add(q,i,j); q_add(q,i,j); q_add(q,i,j); snakelength += 3; } else { q_add(q,i,j); q_add(q,i,j); q_add(q,i,j); q_add(q,i,j); snakelength += 4; } if (snakelength < 720) { random_x = food_x(); random_y = food_y(); // if the snake is already there, find a new spot for the food while (grid[random_x/5][random_y/5-1] == 1) { random_x = food_x(); random_y = food_y(); } // print the new food tft.fillRect(random_x, random_y, 5, 5, YELLOW); } } // if the snake runs over itself if ((snakelength > 1) && (grid[i/5][j/5-1] == 1)) { delay(450); // pause when snake runs into itself int m = 0; soundLoop(); while(m < 6000) { int rand_x = dissolve_x(); int rand_y = dissolve_y(); tft.fillRect(rand_x, rand_y, 5, 5, BLACK); m++; } state = C3; break; } px = q_frontx(q); py = q_fronty(q); // reprint the snake if there is movement if ((i != px) || (j != py)) { tft.fillRect(i,j,5,5, WHITE); grid[i/5][j/5-1] = 1; // snake body is in grid tft.fillRect(px,py,5,5,DARKGRN); grid[px/5][py/5-1] = 0; // snake body is no longer in grid q_remove(q); // take away from the queue delay(speed); // controls the speed of the snake } // if any of the borders are hit if ((i < 4)||(j < 5)||(i > 119)||(j > 150)) { delay(450); // pause when border is hit // dissolve the screen int m = 0; soundLoop(); while(m < 6000) { int rand_x = dissolve_x(); int rand_y = dissolve_y(); tft.fillRect(rand_x, rand_y, 5, 5, BLACK); m++; } //~ delay(250); state = C3; break; } // Read the Joystick - HIGH if not pressed, LOW otherwise select = digitalRead(JOYSTICK_BUTTON); if (select == LOW) { state = C5; break; } } } else if (state == C3) { /// ====== MODE 3 - GAME OVER ====== /// Serial.println("Currently in Mode 3"); q_destroy(q); // clear the queue tft.fillScreen(BLACK); tft.fillRoundRect(5,20,118,25,5,RED); tft.setCursor(10, 25); tft.setTextColor(BLACK); tft.setTextSize(2); tft.setTextWrap(true); tft.print("GAME OVER"); tft.print("\n"); tft.setCursor(10, 55); tft.setTextColor(RED); tft.setTextSize(1.5); if (snakelength >= 720) { snakelength = 720; tft.print("YOU WON! CONGRATZ"); } else { tft.print(" Oh no! You hit something!"); } tft.setCursor(10, 80); tft.setTextColor(WHITE); tft.setTextSize(1); tft.print("Length of Snake:"); tft.print(snakelength); tft.setCursor(10, 100); tft.print("Press the joystick to return to main menu"); // Read the Joystick - HIGH if not pressed, LOW otherwise while (true) { select = digitalRead(JOYSTICK_BUTTON); if (select == LOW) { break; } } state = C1; } else if (state == C4) { /// ====== MODE 4 - CHOOSE LEVEL ====== /// Serial.println("Currently in Mode 4"); // printing // snake display snake(); // difficulty levels tft.setTextSize(2); tft.setTextColor(WHITE); easy(RED); tft.setTextColor(RED); medium(WHITE); hard(WHITE); int selection = 1; int oldselection; while(true) { // read direction from the user for updating selection oldselection = selection; vertical = analogRead(JOYSTICK_VERT); // will be 0-1023 delay(100); // scroll down if (vertical > init_vert + 200) { selection++; if (selection > 3) { selection = 0; } } // scroll up else if (vertical < init_vert - 200) { selection--; if (selection < 0) { selection = 3; } } if (selection != oldselection) { update(selection); } // Read the Joystick - HIGH if not pressed, LOW otherwise select = digitalRead(JOYSTICK_BUTTON); if (select == LOW) { Serial.print("made selection: "); Serial.println(selection); if (selection == 1) {speed = 225;} else if (selection == 2) {speed = 150;} else if (selection == 3) {speed = 75;} break; } } state = C2; } else if (state == C5) { /// ====== MODE 5 - PAUSE MENU ====== /// Serial.println("Currently in Mode 5"); pausedirection = direction; // printing snake and pause snake(); tft.setTextSize(2); tft.setCursor(34, 73); tft.fillRoundRect(30,70,65,20,5,WHITE); tft.setTextColor(RED); tft.print("Pause"); while(true) { // Read the Joystick - HIGH if not pressed, LOW otherwise select = digitalRead(JOYSTICK_BUTTON); if (select == LOW) { break; } } // reset grid to 0 for (int a = 0; a < 24; a++) { for (int b = 0; b < 30; b++) { grid[a][b] = 0; } } state = C2; } //if not any of this: else { Serial.println("There has been an error"); state = ERR; } } Serial.end(); return 0; }
//This gets the time from the server and sets the system Time. //This function is also used as syncProvider: time_t RRTime::getTime(){ unsigned int localPort = 8888; // local port to listen for UDP packets /* Don't hardwire the IP address or we won't get the benefits of the pool. * Lookup the IP address for the host name instead */ //IPAddress timeServerIP(132, 163, 4, 102); // time-b.timefreq.bldrdoc.gov IPAddress timeServerIP; // time.nist.gov NTP server address const char* ntpServerName = "europe.pool.ntp.org"; static const int NTP_PACKET_SIZE = 48; // NTP time stamp is in the first 48 bytes of the message byte packetBuffer[ NTP_PACKET_SIZE]; //buffer to hold incoming and outgoing packet const int timeZone = 1; // Central European Time WiFiUDP udp; // A UDP instance to let us send and receive packets over UDP udp.begin(localPort); Serial.print("Local port: "); Serial.println(udp.localPort()); Serial.println("waiting for sync"); //get a random server from the pool if(!WiFi.hostByName(ntpServerName, timeServerIP)){ Serial.print("ERROR; Could not resolve IP using "); IPAddress fallBack(132, 163, 4, 102); timeServerIP = fallBack; Serial.println(timeServerIP); } Serial.print("Timesever IP:"); Serial.println(timeServerIP); while (udp.parsePacket() > 0) ; // discard any previously received packets Serial.println("Transmit NTP Request"); // set all bytes in the buffer to 0 ///////////////////////////////////////////////////////////////////////// memset(packetBuffer, 0, NTP_PACKET_SIZE); // Initialize values needed to form NTP request // (see URL above for details on the packets) packetBuffer[0] = 0b11100011; // LI, Version, Mode packetBuffer[1] = 0; // Stratum, or type of clock packetBuffer[2] = 6; // Polling Interval packetBuffer[3] = 0xEC; // Peer Clock Precision // 8 bytes of zero for Root Delay & Root Dispersion packetBuffer[12] = 49; packetBuffer[13] = 0x4E; packetBuffer[14] = 49; packetBuffer[15] = 52; // all NTP fields have been given values, now // you can send a packet requesting a timestamp: udp.beginPacket(timeServerIP, 123); //NTP requests are to port 123 udp.write(packetBuffer, NTP_PACKET_SIZE); udp.endPacket(); ///////////////////////////////////////////////////////////////////////// uint32_t beginWait = millis(); while (millis() - beginWait < 1500) { int size = udp.parsePacket(); if (size >= NTP_PACKET_SIZE) { Serial.println("Receive NTP Response"); udp.read(packetBuffer, NTP_PACKET_SIZE); // read packet into the buffer unsigned long secsSince1900; // convert four bytes starting at location 40 to a long integer secsSince1900 = (unsigned long)packetBuffer[40] << 24; secsSince1900 |= (unsigned long)packetBuffer[41] << 16; secsSince1900 |= (unsigned long)packetBuffer[42] << 8; secsSince1900 |= (unsigned long)packetBuffer[43]; //store last sync: lastSync = millis(); return secsSince1900 - 2208988800UL + timeZone * SECS_PER_HOUR; } } Serial.println("No NTP Response :-("); return 0; // return 0 if unable to get the time }
void TimeKeeper::cycle() { this->currentTime = millis(); this->timeFrame = this->currentTime - this->lastTick; }
void FirmataReporting::reset() { previousMillis = millis(); samplingInterval = 19; }
int SkynetClient::connect(IPAddress ip, uint16_t port){ status = 0; bind = 0; DBGCSN("Connecting TCP"); //connect tcp or fail if (!client->connect(ip, port)) { client->stop(); DBGCSN("TCP Failed"); return false; } client->println(F("POST /socket.io/1/ HTTP/1.1")); client->print(F("Host: ")); client->println(ip); client->println(F("\r\n")); //receive data or return if(!waitSocketData()) { client->stop(); DBGCSN("Post Failed"); return false; } //check for OK or return if(readLine(databuffer, SOCKET_RX_BUFFER_SIZE) == 0 || strstr (databuffer,"200") == NULL){ client->stop(); DBGCSN("No Initial OK response"); return false; } //dump the response until the socketid line for(int i = 0; i<7; i++){ if(readLine(databuffer, SOCKET_RX_BUFFER_SIZE)==0) { client->stop(); DBGCSN("Malformed POST response"); return false; } } //get the sid out of buffer char sid[SID_MAXLEN+1]; int i = 0; while((char)databuffer[i] != ':'){ sid[i]=databuffer[i]; i++; } sid[i++]=0; DBGCS("SID: "); DBGCN(sid); //dump the remaining response while(client->available()) client->read(); client->print(F("GET /socket.io/1/websocket/")); client->print(sid); client->println(F(" HTTP/1.1")); client->print(F("Host: ")); client->println(ip); client->println(F("Upgrade: WebSocket")); client->println(F("Connection: Upgrade")); client->println(F("")); //receive data or return if(!waitSocketData()) { client->stop(); DBGCSN("GET Failed"); return false; } //check for OK or return if(readLine(databuffer, SOCKET_RX_BUFFER_SIZE) == 0 || strstr (databuffer,"101") == NULL) { DBGCSN("No Final OK response"); client->stop(); return false; } DBGCSN("Websocket Connected"); //dump the rest of the response for(int i = 0; i<5; i++){ readLine(databuffer, SOCKET_RX_BUFFER_SIZE); } //havent gotten a heartbeat yet so lets set current time lastBeat = millis(); //monitor to initiate communications with skynet while(!monitor() && (unsigned long)(millis() - lastBeat) <= SOCKETTIMEOUT); return status; }
uint16_t GasSensor::loopAction(char temp, char humidity) { // Quality calculation after MAP_RESET_TIME uint32_t start_millis = millis(); if (start_millis>lmillis) { mapresettime=mapresettime-((start_millis-lmillis)/1000); if (mapresettime<=0) { // reset counter mapresettime = MAP_RESET_TIME; // htmap_max -> htmap_avg calucation for (uint8_t hum=0; hum<HT_MAP_COUNT_HUM; hum++) { for (uint8_t temp=0; temp<HT_MAP_COUNT_TEMP; temp++) { if (htmap_max[hum][temp]<65535) { htmap_avg[hum][temp]=(uint16_t)(((unsigned long)htmap_avg[hum][temp]*(MAP_RESET_AVG_COUNT-1)+(unsigned long)htmap_max[hum][temp])/MAP_RESET_AVG_COUNT); htmap_max[hum][temp]=65535; } } } } } lmillis=start_millis; // Pin protection #ifdef ARDUINO if (pinhigh) { if (!digitalRead(pin)) { pinhigh=false; } else { return 65535; } } #endif // Read value int tmp=analogRead(pin); // Pin protection II Value near maximum (1.2V)? if (tmp>65000) { pinhigh=true; } // Burn in time? if ( (lval==0) && (millis()<SENSOR_GAS_PREHEAT_TIME)) { ltmp = temp; lhum = humidity; return tmp; } // Calculation char hpos = (humidity-HT_MAP_MIN_HUM)/HT_MAP_DIV_HUM; if (hpos<0) hpos==0; if (hpos>=HT_MAP_COUNT_HUM) hpos=HT_MAP_COUNT_HUM-1; char tpos = (temp-HT_MAP_MIN_TEMP)/HT_MAP_DIV_TEMP; if (tpos<0) tpos=0; if (tpos>=HT_MAP_COUNT_TEMP) tpos=HT_MAP_COUNT_TEMP-1; // Write htmap lval = tmp; sethtmap(temp,humidity,tmp); // Rule check to avoid jumps in table, check actual cell against neighbours int maxval = htmap_avg[hpos][tpos]; if ( (hpos>0) && (tpos>0) && (hpos<HT_MAP_COUNT_HUM-1) && (tpos<HT_MAP_COUNT_TEMP-1)) { // hpos-1 must be less if (htmap_avg[hpos-1][tpos]>maxval) htmap_avg[hpos-1][tpos]=maxval; // tpos-1 must be less if (htmap_avg[hpos][tpos-1]>maxval) htmap_avg[hpos][tpos-1]=maxval; // hpos-1,tpos-1 must be less if (htmap_avg[hpos-1][tpos-1]>maxval) htmap_avg[hpos-1][tpos-1]=maxval; // hpos+1 must be greater if (htmap_avg[hpos+1][tpos]<maxval) htmap_avg[hpos][tpos-1]=maxval; // tpos +1 must be greater if (htmap_avg[hpos][tpos+1]<maxval) htmap_avg[hpos][tpos+1]=maxval; // hpos+1,tpos +1 must be greater if (htmap_avg[hpos+1][tpos+1]<maxval) htmap_avg[hpos+1][tpos+1]=maxval; // hpos-1,tpos +1 must be greater if (htmap_avg[hpos-1][tpos+1]<maxval) htmap_avg[hpos-1][tpos+1]=maxval; // hpos+1,tpos-1 must be less if (htmap_avg[hpos+1][tpos-1]>maxval) htmap_avg[hpos+1][tpos-1]=maxval; } // maxval: bilinear interpolation // avoid quality jumping when temp/hum if ( (htmap_avg[hpos][tpos+1]<65535) && (htmap_avg[hpos+1][tpos]<65535) && (htmap_avg[hpos+1][tpos+1]<65535) && (hpos<HT_MAP_COUNT_HUM-2) && (tpos<HT_MAP_COUNT_TEMP-2)) { char modulo_temp = (temp-HT_MAP_MIN_TEMP)%HT_MAP_DIV_TEMP; char modulo_hum = (humidity-HT_MAP_MIN_HUM)%HT_MAP_DIV_HUM; uint16_t temp1 = interpolation(htmap_avg[hpos][tpos],htmap_avg[hpos][tpos+1], modulo_temp, HT_MAP_DIV_TEMP); uint16_t temp2 = interpolation(htmap_avg[hpos+1][tpos],htmap_avg[hpos+1][tpos+1], modulo_temp, HT_MAP_DIV_TEMP); maxval = interpolation(temp1,temp2, modulo_hum, HT_MAP_DIV_HUM); } // quality = ( (float)maxval/(float)lval)*100; float f1 = lval-maxval; if (f1<0) f1=0; float f2 = (maxval*SENSOR_VARIANCE)-maxval; if (f2<0) f2=1; quality = 100-((f1*100)/f2); ltmp = temp; lhum = humidity; // Store quality history quality_history[quality_history_pos]=quality; quality_history_pos++; if (quality_history_pos>=QUALITY_HISTORY) quality_history_pos=0; return lval; }
void AnalogSampler::endSample() { _endTime = millis(); }
void UI::poll(time_t t) { // First, refresh whichever state we are in. // If showing clock, move the hands uint32_t polled_millis = millis(); if (showing_clock_) { if (polled_millis - last_clock_millis_ >= 1000) { last_clock_millis_ = polled_millis; d_->clock_poll(t, true, false); } } else { d_->refresh(t, false); } /// XXX if (polled_millis - last_touch_millis_ < kDebounce) { return; } bool selected = false; bool moved = false; bool moved_up = false; uint8_t sel = digitalRead(kRotarySelect); int32_t pos = enc_->read(); // tft->setFont(NULL); // tft->setCursor(0, 0); // tft->println(sel); // tft->println(pos); // tft->println(enc_position_); delay(1); if (sel == 0 && select_ == 1) { // falling edge! selected = true; } select_ = sel; if (pos < (enc_position_ - kRotationTolerance)) { moved = true; moved_up = false; } if (pos > (enc_position_ + kRotationTolerance)) { moved = true; moved_up = true; } enc_position_ = pos; switch (t_->poll()) { case Touch::NO_INFO: break; case Touch::CLICK: #ifdef DEBUG Serial.println("T_CLICK"); #endif if (t_->last_y() > 120) { selected = true; } break; case Touch::LEFT: #ifdef DEBUG Serial.println("T_LEFT"); #endif moved = true; moved_up = true; break; case Touch::RIGHT: #ifdef DEBUG Serial.println("T_RIGHT"); #endif moved = true; moved_up = false; break; case Touch::UP: #ifdef DEBUG Serial.println("T_UP"); #endif moved = true; moved_up = false; // the cursor moves down! break; case Touch::DOWN: #ifdef DEBUG Serial.println("T_DOWN"); #endif moved = true; moved_up = true; // the cursor moves up! break; case Touch::MAYBE_LEFT: #ifdef DEBUG Serial.println("MAYBE_LEFT"); #endif break; case Touch::MAYBE_RIGHT: #ifdef DEBUG Serial.println("MAYBE_RIGHT"); #endif break; case Touch::MAYBE_UP: #ifdef DEBUG Serial.println("MAYBE_UP"); #endif break; case Touch::MAYBE_DOWN: #ifdef DEBUG Serial.println("MAYBE_DOWN"); #endif break; default: Serial.println("PANIC"); } delay(1); if (!moved && !selected) { // If nothing has been touched for a while, and we are still showing // the otp screen, switch back to showing the clock. if (!showing_clock_ && (polled_millis - last_touch_millis_ > kIdleTimeout)) { showing_clock_ = true; d_->clock_poll(t, true, true); } return; } last_touch_millis_ = polled_millis; #ifdef DEBUG Serial.println("last touch millis: " + String(last_touch_millis_)); #endif if (showing_clock_) { showing_clock_ = false; d_->refresh(t, true); return; } if (selected) { d_->select(); #ifdef DEBUG Serial.println("SELECT"); #endif } else if (moved && moved_up) { d_->up(); } else if (moved && !moved_up) { d_->down(); } }
void MyWiegand::reset() { bitHolder = 0; bitCount = 0; wiegandTimeout = millis(); }
UI::UI() : showing_clock_(true), last_clock_millis_(millis()) { }
void main(void) { clock_prescale_set(clock_div_1); // Timer 0 settings for approx. millisecond tracking TCCR0A = _BV(WGM01); TCCR0B = CLOCKSEL; TIMSK0 = _BV(OCIE0A); OCR0A = TICKS; // UART //PORTD |= _BV(PORTD0); //DDRD |= _BV(DDD1); UBRR0 = UBRR_VALUE; #if USE_2X UCSR0A = _BV(U2X0); #endif UCSR0B = _BV(TXEN0); // LED indicator DDRC |= _BV(DDC5); PORTC |= _BV(PORTC5); stdout = &uart_io; // Start up nRF905_init(); // Set address of this device nRF905_setListenAddress(RXADDR); // Interrupts on sei(); uint8_t counter = 0; uint32_t sent = 0; uint32_t replies = 0; uint32_t timeouts = 0; uint32_t invalids = 0; while(1) { // Make data char data[NRF905_MAX_PAYLOAD] = {0}; sprintf_P(data, PSTR("test %hhu"), counter); counter++; packetStatus = PACKET_NONE; printf_P(PSTR("Sending data: %s\n"), data); uint32_t startTime = millis(); // Send the data (send fails if other transmissions are going on, keep trying until success) and enter RX mode on completion while(!nRF905_TX(TXADDR, data, sizeof(data), NRF905_NEXTMODE_RX)); sent++; puts_P(PSTR("Data sent, waiting for reply...")); uint8_t success; // Wait for reply with timeout uint32_t sendStartTime = millis(); while(1) { success = packetStatus; if(success != PACKET_NONE) break; else if(millis() - sendStartTime > TIMEOUT) break; } if(success == PACKET_NONE) { puts_P(PSTR("Ping timed out")); timeouts++; } else if(success == PACKET_INVALID) { // Got a corrupted packet puts_P(PSTR("Invalid packet!")); invalids++; } else { // If success toggle LED and send ping time over UART uint16_t totalTime = millis() - startTime; PORTC ^= _BV(PORTC5); replies++; printf_P(PSTR("Ping time: %ums\n"), totalTime); // Get the ping data uint8_t replyData[NRF905_MAX_PAYLOAD]; nRF905_read(replyData, sizeof(replyData)); // Print out ping contents printf_P(PSTR("Data from server: ")); for(uint8_t i=0;i<sizeof(replyData);i++) printf_P(PSTR("%c"), replyData[i]); puts_P(PSTR("")); } printf_P(PSTR("Totals: %lu Sent, %lu Replies, %lu Timeouts, %lu Invalid\n------\n"), sent, replies, timeouts, invalids); _delay_ms(1000); } }
bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins) { bool detected = false; #ifdef STM32F10X // enable AFIO for EXTI support RCC_ClockCmd(RCC_APB2(AFIO), ENABLE); #endif #if defined(STM32F3) || defined(STM32F4) RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this? #endif triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag); echoIO = IOGetByTag(rangefinderHardwarePins->echoTag); if (IOGetOwner(triggerIO) != OWNER_FREE) { return false; } if (IOGetOwner(echoIO) != OWNER_FREE) { return false; } // trigger pin IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0); IOConfigGPIO(triggerIO, IOCFG_OUT_PP); IOLo(triggerIO); delay(100); // echo pin IOInit(echoIO, OWNER_SONAR_ECHO, 0); IOConfigGPIO(echoIO, IOCFG_IN_FLOATING); // HC-SR04 echo line should be low by default and should return a response pulse when triggered if (IORead(echoIO) == false) { for (int i = 0; i < 5 && !detected; i++) { timeMs_t requestTime = millis(); hcsr04_start_reading(); while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) { if (IORead(echoIO) == true) { detected = true; break; } } } } if (detected) { // Hardware detected - configure the driver #ifdef USE_EXTI EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler); EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority! EXTIEnable(echoIO, true); #endif dev->delayMs = 100; dev->maxRangeCm = HCSR04_MAX_RANGE_CM; dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES; dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES; dev->init = &hcsr04_init; dev->update = &hcsr04_update; dev->read = &hcsr04_get_distance; return true; } else { // Not detected - free resources IORelease(triggerIO); IORelease(echoIO); return false; } }
void Rover::resetPerfData(void) { mainLoop_count = 0; G_Dt_max = 0; perf_mon_timer = millis(); }
int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv) { init_sonar(); delay(20); sonar.update(); if (sonar.status() == RangeFinder::RangeFinder_NotConnected) { cliSerial->println("WARNING: Sonar is not enabled"); } print_hit_enter(); float sonar_dist_cm_min = 0.0f; float sonar_dist_cm_max = 0.0f; float voltage_min=0.0f, voltage_max = 0.0f; float sonar2_dist_cm_min = 0.0f; float sonar2_dist_cm_max = 0.0f; float voltage2_min=0.0f, voltage2_max = 0.0f; uint32_t last_print = 0; while (true) { delay(20); sonar.update(); uint32_t now = millis(); float dist_cm = sonar.distance_cm(0); float voltage = sonar.voltage_mv(0); if (is_zero(sonar_dist_cm_min)) { sonar_dist_cm_min = dist_cm; voltage_min = voltage; } sonar_dist_cm_max = MAX(sonar_dist_cm_max, dist_cm); sonar_dist_cm_min = MIN(sonar_dist_cm_min, dist_cm); voltage_min = MIN(voltage_min, voltage); voltage_max = MAX(voltage_max, voltage); dist_cm = sonar.distance_cm(1); voltage = sonar.voltage_mv(1); if (is_zero(sonar2_dist_cm_min)) { sonar2_dist_cm_min = dist_cm; voltage2_min = voltage; } sonar2_dist_cm_max = MAX(sonar2_dist_cm_max, dist_cm); sonar2_dist_cm_min = MIN(sonar2_dist_cm_min, dist_cm); voltage2_min = MIN(voltage2_min, voltage); voltage2_max = MAX(voltage2_max, voltage); if (now - last_print >= 200) { cliSerial->printf("sonar1 dist=%.1f:%.1fcm volt1=%.2f:%.2f sonar2 dist=%.1f:%.1fcm volt2=%.2f:%.2f\n", (double)sonar_dist_cm_min, (double)sonar_dist_cm_max, (double)voltage_min, (double)voltage_max, (double)sonar2_dist_cm_min, (double)sonar2_dist_cm_max, (double)voltage2_min, (double)voltage2_max); voltage_min = voltage_max = 0.0f; voltage2_min = voltage2_max = 0.0f; sonar_dist_cm_min = sonar_dist_cm_max = 0.0f; sonar2_dist_cm_min = sonar2_dist_cm_max = 0.0f; last_print = now; } if (cliSerial->available() > 0) { break; } } return (0); }
void ledCube8_Tetris::run() { startTime = millis(); init(); while(true) { time = millis(); but1->checkButton(); but2->checkButton(); but3->checkButton(); but4->checkButton(); but5->checkButton(); but6->checkButton(); but7->checkButton(); but8->checkButton(); if((time -startTime) > shiftTime) { activeObjectA = random(0,7); startTime = time; if(playerIsDeath == false) { shiftDownAmount++; } if(checkForCollision(1,0) && playerIsDeath == false) { shiftDownAmount = 0; cubeTerain = cube->add(cubeTerain,activeObject3); activeObject1 = objects[activeObjectA]; if( cubeTerain.CA8 && activeObject3.CA8 >= 1 || cubeTerain.CB8 && activeObject3.CB8 >= 1 || cubeTerain.CC8 && activeObject3.CC8 >= 1 || cubeTerain.CD8 && activeObject3.CD8 >= 1 || cubeTerain.CE8 && activeObject3.CE8 >= 1 || cubeTerain.CF8 && activeObject3.CF8 >= 1 || cubeTerain.CG8 && activeObject3.CG8 >= 1 || cubeTerain.CH8 && activeObject3.CH8 >= 1 ) { playerIsDeath = true; } if( playerIsDeath == false) { shiftDownAmount = 0; shiftLeftAmount = 3; shiftYAmount = 3; rotationXAmount = 0; rotationYAmount = 0; rotationZAmount = 0; activeObject1 = objects[activeObjectA]; activeObjectOnlyRotate = activeObject1; score++; checkForDelete(); } } calculate(); send(); /*p_lcd->clear(); p_lcd-> setCursor (0, 0); p_lcd-> print ("Player 1: "); p_lcd-> print (scoreP1); p_lcd-> setCursor (0, 1); p_lcd-> print ("Player 2: "); p_lcd-> print (scoreP2); send();*/ } //checkForDelete(); if(playerIsDeath == true) { Serial.println("******************************"); Serial.print("Score: "); Serial.println(score); p_lcd-> clear(); p_lcd-> setCursor (0, 0); p_lcd-> print ("Score: "); p_lcd-> print (score); p_lcd-> setCursor (0, 1); stop(); return; } send(); } }
int ModbusSlave::update(int *regs, unsigned int regs_size) { unsigned char query[MAX_MESSAGE_LENGTH]; unsigned char errpacket[EXCEPTION_SIZE + CHECKSUM_SIZE]; unsigned int start_addr; int exception; int length = Serial.available(); unsigned long now = millis(); if (length == 0) { lastBytesReceived = 0; return 0; } if (lastBytesReceived != length) { lastBytesReceived = length; Nowdt = now + T35; return 0; } if (now < Nowdt) return 0; lastBytesReceived = 0; length = modbus_request(query); if (length < 1) return length; exception = validate_request(query, length, regs_size); if (exception) { build_error_packet( query[FUNC], exception, errpacket); send_reply(errpacket, EXCEPTION_SIZE); return (exception); } start_addr = ((int) query[START_H] << 8) + (int) query[START_L]; switch (query[FUNC]) { case FC_READ_REGS: return read_holding_registers( start_addr, query[REGS_L], regs); break; case FC_WRITE_REGS: return preset_multiple_registers( start_addr, query[REGS_L], query, regs); break; case FC_WRITE_REG: write_single_register( start_addr, query, regs); break; } }
int SkynetClient::monitor() { //if we've expired, reconnect to skynet at least if(status == 1 && (unsigned long)(millis() - lastBeat) >= HEARTBEATTIMEOUT){ DBGCS("Timeout: "); DBGCN(millis()); DBGCS("lastbeat: "); DBGCN(lastBeat); stop(); return status; } flush(); if (client->available()) { int size = readLine(databuffer, SOCKET_RX_BUFFER_SIZE); char *first = strchr(databuffer, ':'); char *second = strchr(first+1, ':'); char *third = strchr(second+1, ':'); //-2 for the colons int ackSize = second - first - 2; char ack[MAXACK+1]; //if first and second colon aren't next to eachother, and acksize is sane, grab the ack character(s) if (ackSize>0 && ackSize<MAXACK) { DBGCN(ackSize); DBGCN(first+1); strncpy(ack, first+1, ackSize); ack[ackSize] = '\0'; DBGCS("ack: "); DBGCN(ack); } //where json parser should start char *dataptr = third+1; char socketType = databuffer[0]; switch (socketType) { //disconnect case '0': DBGCSN("Disconnect"); stop(); break; //messages case '1': DBGCSN("Socket Connect"); break; case '3': DBGCSN("Data"); b64::decodestore(dataptr, rxbuf); break; case '5': DBGCSN("Message"); processSkynet(dataptr, ack); break; //hearbeat case '2': DBGCS("Heartbeat at: "); lastBeat = millis(); DBGCN(lastBeat); client->print((char)0); client->print(F("2::")); client->print((char)255); break; //huh? default: DBGCS("Drop: "); DBGCN(socketType); break; } } return status; }
void PID_autotune(float temp, int extruder, int ncycles) { float input = 0.0; int cycles=0; bool heating = true; unsigned long temp_millis = millis(); unsigned long t1=temp_millis; unsigned long t2=temp_millis; long t_high = 0; long t_low = 0; long bias, d; float Ku, Tu; float Kp, Ki, Kd; float max = 0, min = 10000; if ((extruder > EXTRUDERS) #if (TEMP_BED_PIN <= -1) ||(extruder < 0) #endif ){ SERIAL_ECHOLN("PID Autotune failed. Bad extruder number."); return; } SERIAL_ECHOLN("PID Autotune start"); disable_heater(); // switch off all heaters. if (extruder<0) { soft_pwm_bed = (MAX_BED_POWER)/2; bias = d = (MAX_BED_POWER)/2; } else { soft_pwm[extruder] = (PID_MAX)/2; bias = d = (PID_MAX)/2; } for(;;) { if(temp_meas_ready == true) { // temp sample ready updateTemperaturesFromRawValues(); input = (extruder<0)?current_temperature_bed:current_temperature[extruder]; max=max(max,input); min=min(min,input); if(heating == true && input > temp) { if(millis() - t2 > 5000) { heating=false; if (extruder<0) soft_pwm_bed = (bias - d) >> 1; else soft_pwm[extruder] = (bias - d) >> 1; t1=millis(); t_high=t1 - t2; max=temp; } }
// perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // integrate the accel vector in the earth frame between GPS readings _ra_sum += _dcm_matrix * (_accel_vector * deltat); // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps()) { // no GPS, or no lock. We assume zero velocity. This at // least means we can cope with gyro drift while sitting // on a bench with no GPS lock if (_ra_deltat < 0.2) { // not enough time has accumulated return; } float airspeed; if (_airspeed && _airspeed->use()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = millis(); _have_gps_lock = false; // update position delta for get_position() _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } else { if (_gps->last_fix_time == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0); last_correction_time = _gps->last_fix_time; if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // remember position for get_position() _last_lat = _gps->latitude; _last_lng = _gps->longitude; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we update using // dead-reckoning from then on _have_position = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } /* * The barometer for vertical velocity is only enabled if we got * at least 5 pressure samples for the reading. This ensures we * don't use very noisy climb rate data */ if (_baro_use && _barometer != NULL && _barometer->get_pressure_samples() >= 5) { // Z velocity is down velocity.z = -_barometer->get_climb_rate(); } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; float v_scale = gps_gain.get()/(_ra_deltat*_gravity); Vector3f vdelta = (velocity - _last_velocity) * v_scale; // limit vertical acceleration correction to 0.5 gravities. The // barometer sometimes gives crazy acceleration changes. vdelta.z = constrain(vdelta.z, -0.5, 0.5); GA_e = Vector3f(0, 0, -1.0) + vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information return; } // calculate the error term in earth frame. Vector3f GA_b = _ra_sum / (_ra_deltat * _gravity); float length = GA_b.length(); if (length > 1.0) { GA_b /= length; if (GA_b.is_inf()) { // wait for some non-zero acceleration information return; } } Vector3f error = GA_b % GA_e; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = sqrt(sq(GA_e.x) + sq(GA_e.y)); // equation 11 float theta = atan2(GA_b.y, GA_b.x); // equation 12 Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z); // step 6 error = GA_b % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (_compass && _compass->use_for_yaw()) { if (have_gps() && gps_gain == 1.0) { error.z *= sin(fabs(roll)); } else { error.z = 0; } } // convert the error term to body frame error = _dcm_matrix.mul_transpose(error); if (error.is_nan() || error.is_inf()) { // don't allow bad values check_matrix(); return; } _error_rp_sum += error.length(); _error_rp_count++; // base the P gain on the spin rate float spin_rate = _omega.length(); // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error * _P_gain(spin_rate) * _kp; if (_fast_ground_gains) { _omega_P *= 8; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step _ra_sum.zero(); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; if (_have_gps_lock && _fly_forward) { // update wind estimate estimate_wind(velocity); } }
void processRx(void) { static bool armedBeeperOn = false; calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) { failsafeStartMonitoring(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle); rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(rxConfig()); /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */ if (throttleStatus == THROTTLE_LOW) { if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) { if (rollPitchStatus == CENTERED) { ENABLE_STATE(ANTI_WINDUP); } else { DISABLE_STATE(ANTI_WINDUP); } } else { pidResetErrorAngle(); pidResetErrorGyro(); } } else { DISABLE_STATE(ANTI_WINDUP); } // When armed and motors aren't spinning, do beeps and then disarm // board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) ) { if (isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if (armingConfig()->auto_disarm_delay != 0 && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over mwDisarm(); armedBeeperOn = false; } else { // still armed; do warning beeps while armed beeper(BEEPER_ARMED); armedBeeperOn = true; } } else { // throttle is not low if (armingConfig()->auto_disarm_delay != 0) { // extend disarm time disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; } if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } else { // arming is via AUX switch; beep while throttle low if (throttleStatus == THROTTLE_LOW) { beeper(BEEPER_ARMED); armedBeeperOn = true; } else if (armedBeeperOn) { beeperSilence(); armedBeeperOn = false; } } } processRcStickPositions(rxConfig(), throttleStatus, armingConfig()->retarded_arm, armingConfig()->disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(modeActivationProfile()->modeActivationConditions); if (!cliMode) { updateAdjustmentStates(adjustmentProfile()->adjustmentRanges); processRcAdjustments(currentControlRateProfile, rxConfig()); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) || (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) { releaseSharedTelemetryPorts(); } else { // the telemetry state must be checked immediately so that shared serial ports are released. telemetryCheckState(); mspAllocateSerialPorts(); } } #endif }
bool TempSensors::readTemp(int index) { DeviceAddress& deviceAddress = _tempSensors[index]._deviceAddress; float t = dallasTemperature.getTempC(deviceAddress); return _tempSensors[index].value.newValue(t, millis()); }
//SUSPENDSOCKET(): suspends listing to socket,socket can still receive data till //a SH command is issued to shut the socket bool gsmGPRS::suspendSocket(){ telitPort.write("+++"); // escape sequence uint64_t startTime = millis(); while ((millis() - startTime) < 2000); // block 2 seconds SET WITH "gaurd time/S12" return catchTelitData(2000,1); }
void Metro::reset() { this->previous_millis = millis(); }
void SoftwareServo::refresh() { uint8_t count = 0, i = 0; uint16_t base = 0; SoftwareServo *p; static unsigned long lastRefresh = 0; unsigned long m = millis(); // if we haven't wrapped millis, and 20ms have not passed, then don't do anything if ( m >= lastRefresh && m < lastRefresh + 20) return; lastRefresh = m; for ( p = first; p != 0; p = p->next ) if ( p->pulse0) count++; if ( count == 0) return; // gather all the SoftwareServos in an array SoftwareServo *s[count]; for ( p = first; p != 0; p = p->next ) if ( p->pulse0) s[i++] = p; // bubblesort the SoftwareServos by pulse time, ascending order for(;;) { uint8_t moved = 0; for ( i = 1; i < count; i++) { if ( s[i]->pulse0 < s[i-1]->pulse0) { SoftwareServo *t = s[i]; s[i] = s[i-1]; s[i-1] = t; moved = 1; } } if ( !moved) break; } // turn on all the pins // Note the timing error here... when you have many SoftwareServos going, the // ones at the front will get a pulse that is a few microseconds too long. // Figure about 4uS/SoftwareServo after them. This could be compensated, but I feel // it is within the margin of error of software SoftwareServos that could catch // an extra interrupt handler at any time. for ( i = 0; i < count; i++) digitalWrite( s[i]->pin, 1); uint8_t start = TCNT0; uint8_t now = start; uint8_t last = now; // Now wait for each pin's time in turn.. for ( i = 0; i < count; i++) { uint16_t go = start + s[i]->pulse0; // loop until we reach or pass 'go' time for (;;) { now = TCNT0; if ( now < last) base += 256; last = now; if ( base+now > go) { digitalWrite( s[i]->pin,0); break; } } } }
unsigned long DMXSerialClass2::noDataSince() { return(millis() - _gotLastPacket);}
/** * Tell the robot to execute a function based on the value of testParam. * Returns true when it has finished the test. And then it stops. */ boolean ASEE2015::go() { static boolean completed = false; static boolean printed = false; unsigned long currentTime = millis(); if (!completed) { if (_eyes) _eyes->update(currentTime); if (_gyro) _gyro->update(currentTime); if (!_readyToGo) { setup(currentTime); } else { switch (mode) { case 1: if (!printed) { Serial.println("Final test of the robot! This is the final product. Goes around the track and"); Serial.println("picks up all the fish, storing them inside. Dumps them all off too."); printed = true; } if (goFinalRun(currentTime)) { completed = true; } break; case 2: if (!printed) { Serial.println("Test PID. Goes to closest fish seen and stops in front of it. "); printed = true; } goToFishAndStop(currentTime); break; case 3: if (!printed) { Serial.println("Test PID and Conveyor.Goes to closest fish seen, picks it up, and stores it"); Serial.println("based on its color."); printed = true; } break; case 4: if (!printed) { Serial.println("Test Conveyor. Picks up a fish and stores it. Has a pattern:"); Serial.println("1,2,3,1, 1,2,3,2, 1,2,3,3 ..."); printed = true; } testConveyor(currentTime, false); break; case 5: if (!printed) { Serial.println("Test fishCollection Route. Goes to closest fish seen, picks it up, and stores it"); Serial.println("based on its color. Then rotates to next fish and repeats for all the fish."); printed = true; } if (collectFish(currentTime)) //collect fish { Serial.println("Collected all fish!"); completed = true; } break; case 6: if (!printed) { Serial.println("Test Conveyor.Picks up a fish, determines color using pixy, and stores it."); printed = true; } testConveyor(currentTime, true); break; case 7: if (!printed) { Serial.println("Test PID. Goes to closest fish seen and stops in front of it, rotates to next one."); printed = true; } if (testPIDRotate(currentTime)) { completed = true; } break; case 8: if (!printed) { Serial.println("Test Conveyor rotator. Rotates the conveyor up for 5 sec and down for 5 sec. "); printed = true; } testConveyorRotate(currentTime); break; case 9: if (!printed) { Serial.println("Test bin dumping."); printed = true; } if (testBinDumping(currentTime)) { completed = true; } break; case 10: if (!printed) { Serial.println("Test bin dumping going around the track"); printed = true; } if (dumpFish(currentTime)) { completed = true; } break; } } } return completed; }
void loop() { // read the potentiometer position colorVal = analogRead(potPin); powerVal = map(analogRead(pwrPotPin), 0, 1023, 0, 510); Serial.println(powerVal, DEC); buttonState = digitalRead(switchPin); // read input value and store it in val if (buttonState != lastButtonState) { if (buttonState == HIGH) { // check if the button is pressed switch(lightMode) { case 0: lightMode = 1; break; case 1: lightMode = 2; break; case 2: lightMode = 3; break; case 3: lightMode = 0; break; } } lastButtonState = buttonState; } if (lightMode == 0) { // turn light off analogWrite(ledRed, 0); analogWrite(ledGreen, 0); analogWrite(ledBlue, 0); } if (lightMode == 1) { // set fixed color setColor(); setPower(); writeLED(); } if (lightMode == 2) { // pulse fixed color if (millis() - previousMillis > interval) { // save the last time you blinked the LED previousMillis = millis(); setColor(); /* if (pulseState > powerVal) { pulseState = powerVal; } */ if (pulseDir == 0) { pulseState--; if (pulseState == 0) { pulseDir = 1; } } else { pulseState++; if (pulseState >= 255) { pulseDir = 0; } } redPwr = map(redPwr, 0, 255, 0, pulseState); bluePwr = map(bluePwr, 0, 255, 0, pulseState); greenPwr = map(greenPwr, 0, 255, 0, pulseState); writeLED(); } } if (lightMode == 3) { // randomsize colorNew and step colorPwr to it if (millis() - previousMillis > interval) { // save the last time you blinked the LED previousMillis = millis(); if (redPwr > redNew) { redPwr--; } if (redPwr < redNew) { redPwr++; } if (greenPwr > greenNew) { greenPwr--; } if (greenPwr < greenNew) { greenPwr++; } if (bluePwr > blueNew) { bluePwr--; } if (bluePwr < blueNew) { bluePwr++; } // If all Pwr match New get new colors if (redPwr == redNew) { if (greenPwr == greenNew) { if (bluePwr == blueNew) { redNew = random(254); greenNew = random(254); blueNew = random(254); } } } writeLED(); } } }
uint8_t Sd2Card::init(uint8_t sckRateID, uint8_t chipSelectPin) { #endif errorCode_ = inBlock_ = partialBlockRead_ = type_ = 0; chipSelectPin_ = chipSelectPin; // 16-bit init start time allows over a minute uint16_t t0 = (uint16_t)millis(); uint32_t arg; // set pin modes pinMode(chipSelectPin_, OUTPUT); digitalWrite(chipSelectPin_, HIGH); #ifndef USE_SPI_LIB pinMode(SPI_MISO_PIN, INPUT); pinMode(SPI_MOSI_PIN, OUTPUT); pinMode(SPI_SCK_PIN, OUTPUT); #endif #ifndef SOFTWARE_SPI #ifndef USE_SPI_LIB // SS must be in output mode even it is not chip select pinMode(SS_PIN, OUTPUT); digitalWrite(SS_PIN, HIGH); // disable any SPI device using hardware SS pin // Enable SPI, Master, clock rate f_osc/128 SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR1) | (1 << SPR0); // clear double speed SPSR &= ~(1 << SPI2X); #else // USE_SPI_LIB SPI.begin(); settings = SPISettings(250000, MSBFIRST, SPI_MODE0); #endif // USE_SPI_LIB #endif // SOFTWARE_SPI // must supply min of 74 clock cycles with CS high. #ifdef USE_SPI_LIB SPI.beginTransaction(settings); #endif for (uint8_t i = 0; i < 10; i++) spiSend(0XFF); #ifdef USE_SPI_LIB SPI.endTransaction(); #endif chipSelectLow(); // command to go idle in SPI mode while ((status_ = cardCommand(CMD0, 0)) != R1_IDLE_STATE) { if (((uint16_t)(millis() - t0)) > SD_INIT_TIMEOUT) { error(SD_CARD_ERROR_CMD0); goto fail; } } // check SD version if ((cardCommand(CMD8, 0x1AA) & R1_ILLEGAL_COMMAND)) { type(SD_CARD_TYPE_SD1); } else { // only need last byte of r7 response for (uint8_t i = 0; i < 4; i++) status_ = spiRec(); if (status_ != 0XAA) { error(SD_CARD_ERROR_CMD8); goto fail; } type(SD_CARD_TYPE_SD2); } // initialize card and send host supports SDHC if SD2 arg = type() == SD_CARD_TYPE_SD2 ? 0X40000000 : 0; while ((status_ = cardAcmd(ACMD41, arg)) != R1_READY_STATE) { // check for timeout if (((uint16_t)(millis() - t0)) > SD_INIT_TIMEOUT) { error(SD_CARD_ERROR_ACMD41); goto fail; } } // if SD2 read OCR register to check for SDHC card if (type() == SD_CARD_TYPE_SD2) { if (cardCommand(CMD58, 0)) { error(SD_CARD_ERROR_CMD58); goto fail; } if ((spiRec() & 0XC0) == 0XC0) type(SD_CARD_TYPE_SDHC); // discard rest of ocr - contains allowed voltage range for (uint8_t i = 0; i < 3; i++) spiRec(); } chipSelectHigh(); #ifndef SOFTWARE_SPI return setSckRate(sckRateID); #else // SOFTWARE_SPI return true; #endif // SOFTWARE_SPI fail: chipSelectHigh(); return false; }