//Called by position system to store x and y coordinates
//does need the TwoIntLogEntry
public void write(LogEntry log_entry)
{
TwoIntLogEntry ti_entry = (TwoIntLogEntry)log_entry;
Tuple <int, int> msg = ti_entry.Message;
_list.Add(msg);
}
//Called by position system to store x and y coordinates
//does need the TwoIntLogEntry
public void write(LogEntry log_entry)
{
TwoIntLogEntry ti_entry = (TwoIntLogEntry)log_entry;
Tuple <int, int> msg = ti_entry.Message;
Console.WriteLine("X: " + msg.Item1.ToString() + " Y: " + msg.Item2.ToString());
}
public void write(LogEntry log_entry)
{
lock (_lock){
TwoIntLogEntry ti_entry = (TwoIntLogEntry)log_entry;
Tuple <int, int> msg = ti_entry.Message;
_cord = new Tuple <int, int>(msg.Item1, msg.Item2);
}
}
//Called by position system to store x and y coordinates
//does need the TwoIntLogEntry
public void write(LogEntry log_entry)
{
TwoIntLogEntry ti_entry = (TwoIntLogEntry)log_entry;
Tuple <int, int> msg = ti_entry.Message;
Tuple <double, double> tmp = new Tuple <double, double>((double)msg.Item1, (double)msg.Item2);
_list.Add(tmp);
}
//Finds the x,y position based on the serial input from the MarvelMind.
//Runs in its own thread to be constantly updating
//This code is a translation from the Arduino sketch given to us MarvelMind Robotics
private void UpdatePositionLoop()
{
//header for packet, to make sure we are receiving things correctly
//these values are all from the MarvelMind Serial Packet information
byte[] headgehog_packet_header = { 0xff, 0x47, 0x01, 0x00, 0x10 };
byte[] packet = new byte[30];//stores the actual packet with some extra room for saftey
int total_reviced = 0;
bool packet_recived = false;
int packet_index = 0;
int header_index = 0;
while (_running)
{
byte b = _b_reader.getData();
//read byte and store into byte_buf
total_reviced++;
//check to see if we are looking for the header still
if (total_reviced <= 5)
{
//if we have not read in the full header yet and the input value is not the value expected
//for the current header position
if (b != headgehog_packet_header[header_index])
{
//if we read a byte that does not belong in the header we restart
header_index = 0;
packet_index = 0;
total_reviced = 0;
//continue; //loop back and try to make sure we start at a correct packet
}
else //we have a valid byte for the header, add it to the packet
{
packet[packet_index] = b;
header_index++;
packet_index++;
}
}
else
{
//we have received the full header
packet[packet_index] = b;
packet_index++;
}
//check to see if we have received everything
if (total_reviced == _packet_size) //total size is 23
//if we got everything, let us know we received the packed, and reset offsets
{
packet_recived = true;
packet_index = 0;
header_index = 0;
total_reviced = 0;
}
if (true == packet_recived)
{
//update the x and y
//get the bytes at positions 9 and 10 and set them to X
setX((short)(packet[9] + (packet[10] << 8)));
//get the bytes at positions 9 and 10 and set them to Y
setY((short)(packet[11] + (packet[12] << 8)));
IntLogEntry ie1 = new IntLogEntry(X);
IntLogEntry ie2 = new IntLogEntry(Y);
TwoIntLogEntry tie = new TwoIntLogEntry(ie1, ie2);
_logger.write(tie);
//This makes stuff work. DO NOT REMOVE UNLESS YOU KNOW WHAT YOU ARE DOING
//....since we don't. But honestly with out it, this system doesn't seem
//to work properly. Probably because it is reading too fast for info to be
//processed by the other parts of the system
System.Threading.Thread.Sleep(10);
//reset packet received since just produced the packet and are waiting for the next one
packet_recived = false;
}
if (false == BytesToRead)
{
_running = false;
}
} //while(_running)
} //end updatePosition()
