The code really belongs in between code tags:
[code]
... your code here...
[/code]
...so it looks like this:
const int PING_MAP[] = {32, 33, 34, 35, 36, 37, 38, 39, 40, 41};
const char input_template[] = "<+##,+##>";
void setup()
{
Serial.begin(9600);
}
void loop()
{
// Get serial input from RPi
String serial_input = get_serial_in();
// Process serial input from RPi
process_serial_in(serial_input);
// Process local data to send to RPi
String serial_output = process_serial_out();
// Send serial data to RPi
send_serial_out(serial_output);
}
String get_serial_in()
{
Serial.println("Getting serial buffer");
// Wait until we have a command to execute
while (true)
{
// If at least one command is in the serial buffer
if (Serial.available() >= sizeof(input_template) - 1)
{
// If there's two or more commands in the buffer
// Get rid of all but one
while (Serial.available() > 2*sizeof(input_template))
{
Serial.read();
}
// If there's less than two commands, grab the full one
char rc;
// Look for the starting character
while ((rc = Serial.read()) != '<')
{
//rc = Serial.read();
}
String serial_string;
// While we haven't read the end character
while ((rc = Serial.read()) != '>')
{
serial_string += rc;
}
// serial_string should now be "+##,+##"
return serial_string;
// Else wait for a full command
}
}
}
void process_serial_in(String serial_input)
{
serial_input.trim();
// serial_input is of the form "+###,+###"
int sign_x = (serial_input[0] == '+') ? 1 : -1;
int sign_z = (serial_input[serial_input.indexOf(',')+1] == '+') ? 1 : -1;
String x_str = serial_input.substring(1,serial_input.indexOf(',')-1);
String z_str = serial_input.substring(serial_input.indexOf(',')+2);
int x = sign_x * x_str.toInt();
int z = sign_z * z_str.toInt();
//Serial.print("X value is ");
//Serial.print(x);
//Serial.print("| Z value is ");
//Serial.println(z);
}
String process_serial_out()
{
String serial_out = "";
for ( int i = 0; i < sizeof(PING_MAP)/sizeof(int); i++)
{
serial_out += String(i);
serial_out += ',';
serial_out += String(read_ping(PING_MAP)/29/2);
serial_out += ';';
}
return serial_out;
}
void send_serial_out(String serial_output)
{
Serial.println(serial_output);
}
long read_ping(int pin_number)
{
pinMode(pin_number, OUTPUT);
digitalWrite(pin_number, LOW);
delayMicroseconds(3);
digitalWrite(pin_number, HIGH);
delayMicroseconds(10);
digitalWrite(pin_number, LOW);
pinMode(pin_number, INPUT);
long result = pulseIn(pin_number, HIGH);
delay(25);
return result;
}
void write_motor_commands(int x, int rotz)
{
int left_int = (x + rotz)/100*128 + 127;
int right_int = (x - rotz)/100*128 + 127;
byte left_byte = (byte)left_int;
byte right_byte = (byte)right_int;
write_left_motor(left_byte);
write_right_motor(right_byte);
}
void write_left_motor(byte value)
{
Serial1.write(0xFF);
Serial1.write(1);
Serial1.write(value);
}
void write_right_motor(byte value)
{
Serial1.write(0xFF);
Serial1.write(2);
Serial1.write(value);
}
Basically, you should not use String. Use char arrays instead, and the C string functions to search, compare, concatenate, etc. There are many tutorials for C strings.
The second problem is that your program structure should be more like Serial Input Basics. Right now, you are waiting in one spot for the whole command to arrive. This keeps other things from getting serviced.
Instead the code is not running because it is saying that the left and right Int and byte are not declared.
When I compile it, here are the errors:
Arduino: 1.6.8, Board: "Arduino/Genuino Mega or Mega 2560, ATmega2560 (Mega 2560)"
avr-g++ -c -g -Os -std=gnu++11 tsmit76.ino.cpp -o tsmit76.ino.cpp.o
tsmit76.ino: In function 'String process_serial_out()':
tsmit76:81: error: invalid conversion from 'const int*' to 'int' [-fpermissive]
serial_out += String(read_ping(PING_MAP)/29/2);
^
tsmit76:93: error: initializing argument 1 of 'long int read_ping(int)' [-fpermissive]
long read_ping(int pin_number)
^
exit status 1
invalid conversion from 'const int*' to 'int' [-fpermissive]
For some reason, you are trying to pass an array to read_ping. Maybe you start with an example program and change one thing at a time.
Cheers,
/dev