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