Exemplo n.º 1
0
void ground_map_loop()
{
  // check for new position
  int pos = (int)( get_lat_position_cm() / MAP_RESOLUTION );

  if( pos > last_pos ) {
    //map_sensor_to_index( RIGHT_DISTANCE_SENSOR, mod_pos( pos + MAP_SIZE/2, MAP_SIZE ) );
    ground_map_buffer[mod_pos( pos + MAP_SIZE/2, MAP_SIZE )] = get_distance( RIGHT_DISTANCE_SENSOR );
  } else if( pos < last_pos ) {
    ground_map_buffer[mod_pos( pos - MAP_SIZE/2, MAP_SIZE )] = get_distance( LEFT_DISTANCE_SENSOR );
    //map_sensor_to_index( LEFT_DISTANCE_SENSOR, mod_pos( pos - MAP_SIZE/2, MAP_SIZE ) );
  }
  
  // check if in safe zone
  static bool cur_safe = false;
  
  if( last_pos != pos ) {
    bool new_safe = safe_zone();
    if( new_safe != cur_safe ) {
      cur_safe = new_safe;
      if( new_safe ) {
        Serial.println( "Safe" );
        digitalWrite( 13, HIGH );
        set_lat_action( MOTOR_STOP );
      } else {
        Serial.println( "Not safe" );
        digitalWrite( 13, LOW );
      }
    }
  }
  
  last_pos = pos;
}
Exemplo n.º 2
0
bool safe_zone()
{
  unsigned start_pos = mod_pos( last_pos - SEAT_SIZE/2, MAP_SIZE );
  float min_height = ground_map_buffer[start_pos];
  float max_height = ground_map_buffer[start_pos];
  
  // find MIN and MAX for the seat area
  for( int i = 1; i < SEAT_SIZE; i++ ) {
    unsigned pos_i = (start_pos + i)%MAP_SIZE;
    if( ground_map_buffer[pos_i] < min_height ) {
      min_height = ground_map_buffer[pos_i];
    } else if( ground_map_buffer[pos_i] < max_height ) {
      max_height = ground_map_buffer[pos_i];
    }
  }
  
  float ave_height = (max_height + min_height) / 2.0;
  
  if( ((max_height - min_height) < SAFE_SURFACE_NOISE) &&
      (ave_height < SAFE_MAX_HEIGHT) &&
      (ave_height > SAFE_MIN_HEIGHT) )
  {
    return true;
  }
  return false;
}
Exemplo n.º 3
0
void print_ground_map()
{
  Serial.print( "I: " );
  Serial.println( last_pos );
  for( int i = 0; i < MAP_SIZE; i++ ) {
    Serial.print( " " );
    Serial.println( ground_map_buffer[mod_pos( i + last_pos - MAP_SIZE/2, MAP_SIZE )] );
  }
}
Exemplo n.º 4
0
	MergedCode SequenceSpace::getMergedCode( const ModificationSequence& mod_seq )
	{
		MergedCode merged_code;

		std::string str;

		Branch& bc = gs->getBranchByID(0);

		ModificationSequence::const_iterator iter = mod_seq.find("SO3");
		const ModificationSites& s_sites = iter->second;

		for(size_t i = 0; i < bc.getUnitNum(); i++)
		{
			std::string symbol = bc.getUnitByID(i).getSymbol();

			if(symbol == "DeltaGlcA") {
				str = "D";
			} else if(symbol == "GlcA") {
				str = "G";
			} else if(symbol == "GlcN") { // A, S or H.
				// Check the 2-N position.
				ModificationPosition mod_pos(0, i, 2);
				ModificationSequence::const_iterator ac_iter = mod_seq.find("Ac");
				ModificationSites::const_iterator s_pos_iter = s_sites.find(mod_pos);

				if(ac_iter != mod_seq.end()) {

					const ModificationSites& ac_sites = ac_iter->second;

					ModificationSites::const_iterator ac_pos_iter = ac_sites.find(mod_pos);

					if(ac_pos_iter != ac_sites.end())
						str = "A";
					/*else if(s_pos_iter != s_sites.end())
						str = "S";*/
					else {
						str = "H";
					}
				} else {
					//if(s_pos_iter != s_sites.end())
					//	str = "S";
					//else {
					//	str = "H";
					//}
					str = "H";
				}

			} else if(symbol == "GlcNAc") {
				str = "A";
			} else {
				str = "X"; // Unknown type.
			}
			if(iter == mod_seq.end()) {
				merged_code.push_back(std::make_pair(str,0));
				continue;
			}

			size_t num = 0;

			// Get the number of SO3 on the ring.
			if(str == "D" || str == "G") {
				ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, 2));
				if(pos_iter != s_sites.end()) {
					num = 1;
				}
			} else if(str == "H" || str == "A") {
				for(size_t j = 1; j < bc.getUnitByID(i).getInternalSites().size(); j++)
				{
					ModificationSites::const_iterator pos_iter = s_sites.find(ModificationPosition(0, i, j));
					if(pos_iter != s_sites.end())
						num += 1;
				}
			} else {
				// Do nothing.
			}

			merged_code.push_back(std::make_pair(str,num));

		}

		return merged_code;
	}