void launch_Trajectory()
{
process_received_string("R0.25");
process_received_string("R-0.75");
process_received_string("R0.25"); //5 degrees is 0.0138888 rotations
}
//---------------------------------------------------------------------------------------
// If there are received bytes to process, this function loops through the receive_buffer
// accumulating new bytes (keystrokes) in another buffer for processing.
void check_for_new_bytes_received()
{
/*
The receive_buffer is a ring buffer. The call to serial_check() (you should call prior to this function) fills the buffer.
serial_get_received_bytes is an array index that marks where in the buffer the most current received character resides.
receive_buffer_position is an array index that marks where in the buffer the most current PROCESSED character resides.
Both of these are incremented % (size-of-buffer) to move through the buffer, and once the end is reached, to start back at the beginning.
This process and data structures are from the Pololu library. See examples/serial2/test.c and src/OrangutanSerial/ *
A carriage return from your comm window initiates the transfer of your keystrokes.
All key strokes prior to the carriage return will be processed with a single call to this function (with multiple passes through this loop).
On the next function call, the carriage return is processes with a single pass through the loop.
The menuBuffer is used to hold all keystrokes prior to the carriage return. The "received" variable, which indexes menuBuffer, is reset to 0
after each carriage return.
*/
char menuBuffer[32];
static int received = 0;
int evaluate = 0;
// while there are unprocessed keystrokes in the receive_buffer, grab them and buffer
// them into the menuBuffer
while(serial_get_received_bytes(USB_COMM) != receive_buffer_position)
{
// place in a buffer for processing
menuBuffer[received] = receive_buffer[receive_buffer_position];
print_usb_char( menuBuffer[received] );
#ifdef ECHO2LCD
lcd_goto_xy(0,0);
print("RX: (");
print_long(menuBuffer[received]);
print_character(')');
for (int i=0; i<received; i++)
{
print_character(menuBuffer[i]);
}
#endif
if ( menuBuffer[received] == '\r' )
{
print_usb( "\n" );
evaluate = 1;
}
++received;
// Increment receive_buffer_position, but wrap around when it gets to
// the end of the buffer.
if ( receive_buffer_position == sizeof(receive_buffer) - 1 )
{
receive_buffer_position = 0;
}
else
{
receive_buffer_position++;
}
}
#ifdef ECHO2LCD
lcd_goto_xy(0,1);
print("RX: (");
print_long(received);
print_character(')');
for (int i=0; i<received; i++)
{
print_character(menuBuffer[i]);
}
#endif
// If there were keystrokes processed, check if a menu command
if ( evaluate ) {/*
// if only 1 received, it was MOST LIKELY a carriage return.
// Even if it was a single keystroke, it is not a menu command, so ignore it.
if ( 1 == received ) {
received = 0;
return;
}*/
// Process buffer: terminate string, process, reset index to beginning of array to receive another command
menuBuffer[received] = '\0';
#ifdef ECHO2LCD
lcd_goto_xy(0,1);
print("RX: (");
print_long(received);
print_character(')');
for (int i=0; i<received; i++)
{
print_character(menuBuffer[i]);
}
#endif
process_received_string(menuBuffer);
received = 0;
}
}
void process_Trajectory()
{
if (running_trajectory == false)
return;
if (command_list[0] == GO)
{
command_list[0] == RUN; //start running
maneuver_complete = RUN;
USE_DEGREES = true;
process_received_string("R90");
}
while (1)
{
if ( (fabs(error) < error_threshold) && (maneuver_complete == DONE) )
{
maneuver_complete = false;
command_list[0] = DONE;
command_list[1] = GO;
delay_ms(500);
break;
}
else
{
delay_ms(10);
}
}
if (command_list[1] == GO)
{
command_list[1] == RUN; //start running
maneuver_complete = RUN;
USE_DEGREES = true;
process_received_string("R-270");
}
while (1)
{
if ( (fabs(error) < error_threshold) && (maneuver_complete == DONE) )
{
maneuver_complete = false;
command_list[1] = DONE;
command_list[2] = GO;
delay_ms(500);
break;
}
else
{
delay_ms(10);
}
}
if (command_list[2] == GO)
{
command_list[2] == RUN; //start running
maneuver_complete = RUN;
USE_DEGREES = true;
process_received_string("R-265");
}
while (1)
{
if ( (fabs(error) < error_threshold) && (maneuver_complete == DONE) )
{
maneuver_complete = false;
command_list[2] = DONE;
//command_list[2] = GO;
break;
}
else
{
delay_ms(10);
}
}
//command_list[0] = WAIT;
//command_list[1] = WAIT;
//command_list[2] = WAIT;
running_trajectory = false;
maneuver_complete = WAIT;
if (TARGET_ROT)
length = sprintf( tempBuffer, "Kp:%f Ki:%f Kd:%f Pr:%f Pm:%f T:%d\r\n", Kp, Ki, Kd, goal, rotations, voltage);
if (TARGET_RPM) {
length = sprintf( tempBuffer, "Kp:%f Ki:%f Kd:%f Pr:%f Pm:%f T:%d\r\n", Kp, Ki, Kd, goal, RPM, voltage);
}
print_usb( tempBuffer, length );
length = sprintf( tempBuffer, "C0:%d C1:%d C2:%d C3:%d C4:%d C5:%d Man:%d\r\n", command_list[0],
command_list[1],
command_list[2],
command_list[3],
command_list[4],
command_list[5],
maneuver_complete);
USE_DEGREES = false; //set things back to normal
}
