void print_reset_reason() {
if (G_reset_source & (1<<JTRF)) { print_usb("JTAG Reset\r\n", 12); }
if (G_reset_source & (1<<WDRF)) { print_usb("Watchdog Reset\r\n", 16); }
if (G_reset_source & (1<<BORF)) { print_usb("Brownout Reset\r\n", 16); }
if (G_reset_source & (1<<EXTRF)) { print_usb("External Reset\r\n", 16); }
if (G_reset_source & (1<<PORF)) { print_usb("Power-on Reset\r\n", 16); }
}
// BRTOS version Command
void usb_cmd_ver(char *param)
{
(void)*param;
print_usb("\n\r");
print_usb((CHAR8*)version);
print_usb("\n\r");
}
void log_print_log_row() {
// Used to pass to USB_COMM for serial communication
int length;
char tempBuffer[256];
length = sprintf(tempBuffer, "%d, %ld, %ld, %d, %ld, %ld, %ld\r\n", g_log_ticks, g_der_gain, g_prop_gain, g_velocity, g_target_position, g_current_position, g_motor_output);
print_usb(tempBuffer, length);
}
// Core temperature Command
//extern INT16S temperature;
void usb_cmd_temp(char *param)
{
INT16S temperature_temp;
CHAR8 string[8];
(void)*param;
UserEnterCritical();
//temperature_temp = temperature;
temperature_temp = 0;
UserExitCritical();
PrintDecimal(temperature_temp, string);
print_usb("\n\r");
print_usb((CHAR8*)&string[3]);
print_usb(" degrees");
}
/*****************************************************************************
* Name:
* cmd_help
* In:
* param - pointer to string containing parameters
*
* Out:
* N/A
*
* Description:
* List supported commands.
*
* Assumptions:
* --
*****************************************************************************/
static void usb_cmd_help(char *param)
{
int x;
int y;
char *name;
(void)param;
print_usb("\r\nI understand the following commands:\r\n");
for(x=0; x < usb_n_cmd; x++)
{
print_usb(" ");
name = (char*)usb_cmds[x]->txt;
y = 0;
while(*name)
{
y++;
name++;
}
print_usb((char *)usb_cmds[x]->txt);
for(y; y<MAX_CMD_SIZE; y++)
{
print_usb(" ");
}
print_usb((char *)usb_cmds[x]->help_txt);
print_usb("\r\n");
}
print_usb("\r\n");
}
//------------------------------------------------------------------------------------------
// Initialize serial communication through USB and print menu options
// This immediately readies the board for serial comm
void init_menu() {
//char printBuffer[32];
// Set the baud rate to 9600 bits per second. Each byte takes ten bit
// times, so you can get at most 960 bytes per second at this speed.
serial_set_baud_rate(USB_COMM, 9600);
// Start receiving bytes in the ring buffer.
serial_receive_ring(USB_COMM, receive_buffer, sizeof(receive_buffer));
//memcpy_P( send_buffer, PSTR("USB Serial Initialized\r\n"), 24 );
//snprintf( printBuffer, 24, "USB Serial Initialized\r\n");
//print_usb( printBuffer, 24 );
print_usb( "\r\n\nUSB Serial Initialized\r\n" );
//memcpy_P( send_buffer, MENU, MENU_LENGTH );
print_usb( MENU );
}
// TOP Command (similar to the linux command)
void usb_cmd_top(char *param)
{
(void)*param;
print_usb("\n\r");
#if (COMPUTES_CPU_LOAD == 1)
Transmite_CPU_Load(USE_USB);
#endif
Transmite_Uptime(USE_USB);
Transmite_RAM_Ocupada(USE_USB);
Transmite_Task_Stacks(USE_USB);
}
int main() {
// capture any reset reason
G_reset_source = MCUSR;
// clear the status register by writing ones
// (odd, I know, but that is how it is done)
MCUSR = 0x1F;
// Display on the LCD that serial connection is needed
print("Waiting for");
lcd_goto_xy(0, 1);
print(" serial conn...");
// This init will block if no serial connection present
// so user sees message on LCD to make a connection
init_interface();
// Display the user interface over the serial usb
// connection
serial_check();
print_reset_reason();
print_usb("Welcome to lab 3!\r\n", 19);
print_usage();
print_prompt();
// clear "Waiting for connection" message from the LCD
clear();
// turn on interrupts
sei();
init_motor();
init_encoder();
// set controller for 1000 Hz
init_controller_w_rate(50);
while (1) {
serial_check();
check_for_new_bytes_received();
}
return 0;
}
//------------------------------------------------------------------------------------------
// process_received_byte: Parses a menu command (series of keystrokes) that
// has been received on USB_COMM and processes it accordingly.
// The menu command is buffered in check_for_new_bytes_received (which calls this function).
void process_received_string(const char* buffer)
{
// Used to pass to USB_COMM for serial communication
char tempBuffer[32];
// parse and echo back to serial comm window (and optionally the LCD)
char color;
char op_char;
int value;
int parsed;
cli();
parsed = sscanf(buffer, "%c %c %d", &op_char, &color, &value);
#ifdef ECHO2LCD
lcd_goto_xy(0,0);
printf("Got %c %c %d\n", op_char, color, value);
#endif
sprintf( tempBuffer, "Op:%c C:%c V:%d\r\n", op_char, color, value );
print_usb( tempBuffer );
// convert color to upper and check if valid
color -= 32*(color>='a' && color<='z');
switch (color) {
case 'R':
case 'G':
case 'Y':
case 'A': break;
default:
print_usb( "Bad Color. Try {RGYA}\r\n" );
print_usb( MENU );
return;
}
// Check valid command and implement
switch (op_char) {
// change toggle frequency for <color> LED
case 'T':
case 't':
switch(color) {
case 'R':
set_red_period( value );
sprintf( tempBuffer, "R freq: %d\r\n", value );
print_usb( tempBuffer );
break;
case 'G':
set_green_period( value );
sprintf( tempBuffer, "G freq: %d\r\n", value );
print_usb( tempBuffer );
break;
case 'Y':
set_yellow_period( value );
sprintf( tempBuffer, "Y freq: %d\r\n", value );
print_usb( tempBuffer );
break;
case 'A':
set_red_period( value );
set_green_period( value );
set_yellow_period( value );
sprintf( tempBuffer, "Freq R:%d G:%d Y:%d\r\n", value, value, value );
print_usb( tempBuffer );
break;
default: print_usb("Default in t(color). How?\r\n" );
}
break;
// print counter for <color> LED
case 'P':
case 'p':
switch(color) {
case 'R':
sprintf( tempBuffer, "R toggles: %d\r\n", get_red_toggle_counter() );
print_usb( tempBuffer );
break;
case 'G':
sprintf( tempBuffer, "G toggles: %d\r\n", get_green_toggle_counter() );
print_usb( tempBuffer );
break;
case 'Y':
sprintf( tempBuffer, "Y toggles: %d\r\n", get_yellow_toggle_counter() );
print_usb( tempBuffer );
break;
case 'A':
sprintf( tempBuffer, "Toggles R:%d G:%d Y:%d\r\n", get_red_toggle_counter(), get_green_toggle_counter(), get_yellow_toggle_counter() );
print_usb( tempBuffer );
break;
default: print_usb("Default in p(color). How?\r\n" );
}
break;
// zero counter for <color> LED
case 'Z':
case 'z':
switch(color) {
case 'R':
clr_red_toggle_counter();
sprintf( tempBuffer, "Zero R\r\n" );
print_usb( tempBuffer );
break;
case 'G':
clr_green_toggle_counter();
sprintf( tempBuffer, "Zero G\r\n" );
print_usb( tempBuffer );
break;
case 'Y':
sprintf( tempBuffer, "Zero Y\r\n" );
print_usb( tempBuffer );
clr_yellow_toggle_counter();
break;
case 'A':
clr_red_toggle_counter();
clr_green_toggle_counter();
clr_yellow_toggle_counter();
sprintf( tempBuffer, "Zero All\r\n" );
print_usb( tempBuffer );
break;
default: print_usb("Default in z(color). How?\r\n" );
}
break;
default:
print_usb( "Command does not compute.\r\n" );
} // end switch(op_char)
print_usb( MENU );
sei();
} //end menu()
// A generic function for whenever you want to print to your serial comm window.
// Provide a string and the length of that string. My serial comm likes "\r\n" at
// the end of each string (be sure to include in length) for proper linefeed.
void print_usb_char( char buffer ) {
char local_buf[2];
local_buf[0] = buffer;
local_buf[1] = NULL;
print_usb( local_buf );
}
//---------------------------------------------------------------------------------------
// 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 handle_input(char* input)
{
if (state == START)
{
if (strcmp("set fan\r", input) == 0)
{
print_usb("enter fan speed, 0...40 \n\r");
/* advance state machine */
state = SET_FAN;
}
else if (strcmp("set pump\r", input) == 0)
{
print_usb("enter pump speed, 0...200 \n\r");
/* advance state machine */
state = SET_PUMP;
}
else if (strcmp("set led\r", input) == 0)
{
print_usb("enter led brightness, 0...255 \n\r");
/* advance state machine */
state = SET_LED;
}
else if (strcmp("read temp\r", input) == 0)
{
print_usb("reading temp... \n\r");
/* advance state machine */
state = READ_TEMP;
}
else
{
/* didnt understand string */
print_usb("did not understand command \n\r");
/* dont change state */
state = START;
}
}
else if (state == SET_LED)
{
static uint16_t pwm;
char *garbage = NULL;
pwm = strtol(input, &garbage, 0);
OCR4A = pwm;
print_usb("led set. \n\r");
/* go back to start */
state = START;
}
else if (state == SET_FAN)
{
static uint16_t pwm;
char *garbage = NULL;
pwm = strtol(input, &garbage, 0);
OCR3A = pwm;
print_usb("fan set. \n\r");
/* go back to start */
state = START;
}
else if (state == SET_PUMP)
{
static uint16_t pwm;
char *garbage = NULL;
pwm = strtol(input, &garbage, 0);
OCR4B = pwm;
print_usb("pump set. \n\r");
/* go back to start */
state = START;
}
else if (state == READ_TEMP)
{
uint8_t nSensors;
char maxres_buffer[10];
ow_set_bus(&PIND, &PORTD, &DDRD, PD4);
ow_reset();
/* search for temperature sensors */
nSensors = search_sensors();
/* classify sensors */
classify_sensors(nSensors);
/* measure temperature */
measure_temp(nSensors, temp_eminus4);
DS18X20_format_from_maxres(temp_eminus4[0], maxres_buffer, 10);
print_usb("\n\r ");
print_usb("temp max res 1: ");
print_usb(maxres_buffer);
print_usb("\n\r ");
DS18X20_format_from_maxres(temp_eminus4[1], maxres_buffer, 10);
print_usb("temp max res 2: ");
print_usb(maxres_buffer);
print_usb("\n\r ");
/* go back to start */
state = START;
}
else
{
CDC_Device_SendString(&USB_Interface, "went into unknown state!? \n\r");
state = START;
}
}
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
}
// Reason of Reset Command
void usb_cmd_rst(char *param)
{
(void)*param;
print_usb("\n\r");
Reason_of_Reset(USE_USB);
}
/*****************************************************************************
* Name:
* terminal_proces
* In:
* N/A
*
* Out:
* N/A
*
* Description:
* Main loop of terminal application. gathers input, and executes commands.
*
* Assumptions:
* --
*****************************************************************************/
void usb_terminal_process(void)
{
INT8U data;
char c;
while(1)
{
(void)OSQueuePend(USB, &data, 0);
c=(char)data;
if ((c != '\n') && (c != '\r'))
{
if (c != 0x7F)
{
if (SilentMode == FALSE)
{
while(c!=(char)(*putch)(c)) {};
}
} else
{
if (usb_cmd_line_ndx)
{
while(c!=(char)(*putch)(c)) {};
}
}
}
/* Execute command if enter is received, or usb_cmd_line is full. */
if ((c=='\r') || (usb_cmd_line_ndx == sizeof(usb_cmd_line)-2))
{
int usb_start = usb_skipp_space(usb_cmd_line, 0);
int usb_end = usb_find_word(usb_cmd_line, usb_start);
int usb_x;
/* Separate command string from parameters, and close
parameters string. */
usb_cmd_line[usb_end]=usb_cmd_line[usb_cmd_line_ndx]='\0';
/* Identify command. */
usb_x=usb_find_command(usb_cmd_line+usb_start);
/* Command not found. */
if (usb_x == -1)
{
print_usb("\r\nUnknown command!\r\n");
}
else
{
(*usb_cmds[usb_x]->func)(usb_cmd_line+usb_end+1);
}
usb_cmd_line_ndx=0;
usb_print_prompt();
SetSilentMode(FALSE);
}
else
{ /* Put character to usb_cmd_line. */
if (c=='\b')
{
if (usb_cmd_line_ndx > 0)
{
usb_cmd_line[usb_cmd_line_ndx]='\0';
usb_cmd_line_ndx--;
}
}
else if(c=='\n') {
continue;
}
else
{
if (c == 0x7F)
{
if (usb_cmd_line_ndx)
{
usb_cmd_line[usb_cmd_line_ndx]=0;
usb_cmd_line_ndx--;
usb_cmd_line[usb_cmd_line_ndx]=0;
}
} else
{
usb_cmd_line[usb_cmd_line_ndx++]=c;
}
}
}
if (SilentMode == FALSE)
{
cdc_process();
}
}
}
static void usb_print_greeting(void)
{
print_usb("BRTOS Started!\r\n");
}
/*****************************************************************************
* Name:
* print_prompt
* In:
* N/A
*
* Out:
* N/A
*
* Description:
* Prints the prompt string.
*
* Assumptions:
* --
*****************************************************************************/
static void usb_print_prompt(void)
{
print_usb("\r\n>");
}
void log_print_header() {
int length;
char tempBuffer[256];
length = sprintf(tempBuffer, "Ticks, Kd, Kp, V, Pr, Pm, T\r\n");
print_usb(tempBuffer, length);
}
// Print a string in the terminal
void cmd_echo(char *param)
{
print_usb("\n\r");
print_usb((CHAR8*)param);
}
int main(void) {
delay_ms(100);
init_leds();
init_timers();
init_motors();
// Used to print to serial comm window
char tempBuffer[80];
// Initialization here.
lcd_init_printf(); // required if we want to use printf() for LCD printing
init_menu(); // this is initialization of serial comm through USB
clear(); // clear the LCD
//enable interrupts
sei();
while (1) {
process_Trajectory();
if (updateLCD)
{
//first line
lcd_goto_xy(8,0);
print(" ");
lcd_goto_xy(8,0);
//print_long(global_counts_m1);
sprintf( tempBuffer, "V=%d\r", voltage);
print(tempBuffer);
//next line
lcd_goto_xy(0,1);
sprintf( tempBuffer, "R=%3.2f RPM=%3.2f\r", rotations, RPM);
print(tempBuffer);
updateLCD = false;
}
if (display_values_flag)
{
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);
else 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);
else
length = sprintf( tempBuffer, "Kp:%f Ki:%f Kd:%f Pr:%f Pm:NA T:%d\r\n", Kp, Ki, Kd, goal, voltage);
print_usb( tempBuffer, length );
length = sprintf( tempBuffer, "C0:%d C1:%d C2:%d C3:%d C4:%d C5:%d Man:%d Eq:%d\r\n", command_list[0],
command_list[1],
command_list[2],
command_list[3],
command_list[4],
command_list[5],
maneuver_complete,
equilibrium);
print_usb( tempBuffer, length );
length = sprintf( tempBuffer, "%f, %f, %f\r\n", error, integral, derivative);
print_usb( tempBuffer, length );
display_values_flag = false;
}
if (ready_to_dump_log)
{
ready_to_dump_log = false;
length = sprintf( tempBuffer, "Goal, Kp, Ki, Kd, Rot, RPM, Volt\r\n");
print_usb( tempBuffer, length );
length = sprintf( tempBuffer, "%f, %f, %f, %f, %f, %f, %d\r\n", goal, Kp, Ki, Kd, rotations, RPM, voltage);
print_usb( tempBuffer, length );
int delta = (log_stop_timestep-log_start_timestep);
length = sprintf( tempBuffer, "Time, Error, KpError, KiIntegral, KdDerivative, Rotations, RPM, Voltage\r\n");
print_usb( tempBuffer, length );
float temp_Kp = (Kp == 0.0) ? 1.0 : Kp;
float temp_Ki = (Ki == 0.0) ? 1.0 : Ki;
float temp_Kd = (Kd == 0.0) ? 1.0 : Kd;
float time;
for(int i=0; i<delta; i++)
{
time = (float)((i*dt)*60);
length = sprintf( tempBuffer, "%f %f %f %f %f %f %f %d\r\n", time, //time expressed in seconds
LOG[i].error,
(float)(temp_Kp * LOG[i].error),
(float)(temp_Ki * LOG[i].integral),
(float)(temp_Kd * LOG[i].derivative),
LOG[i].rotations,
LOG[i].RPM,
LOG[i].voltage);
print_usb( tempBuffer, length );
}
}
//Process Control from the USB Port
serial_check();
check_for_new_bytes_received();
} //end while loop
} //end main
//------------------------------------------------------------------------------------------
// process_received_byte: Parses a menu command (series of keystrokes) that
// has been received on USB_COMM and processes it accordingly.
// The menu command is buffered in check_for_new_bytes_received (which calls this function).
void process_received_string(const char* buffer)
{
// Used to pass to USB_COMM for serial communication
int length;
char tempBuffer[80];
// parse and echo back to serial comm window (and optionally the LCD)
char op_cmd;
float op_value = 0.0;
int temp_goal = 0;
//THROTTLE MODE requires setting putty to force sending text immediately
#ifdef THROTTLE_MODE
length = sscanf(buffer, "%c", &op_cmd);
lcd_goto_xy(0,0);
printf("Got %c", op_cmd);
// convert to upper and check if valid
op_cmd -= 32*(op_cmd>='a' && op_cmd<='z');
switch (op_cmd) {
case 'R':
print_usb("Switching direction\r\n",22);
int difference = 2*abs(voltage);
if (voltage > 0)
{
for (int i=0; i<difference; i+=10)
{
voltage -= 10;
delay_ms(50);
//OCR2A = voltage;
//set_motors(voltage, 0);
}
} else {
for (int i=0; i<difference; i+=10)
{
voltage += 10;
delay_ms(50);
//OCR2A = voltage;
//set_motors(voltage, 0);
}
}
break;
case 'S':
print_usb("Stopped\r\n",16);
//motor1_dir = Stop;
voltage = (int16_t)0;
break;
case 'U':
voltage += 10;
if (voltage > 255)
voltage = (int16_t)250;
break;
case 'J':
voltage -= 10;
if (voltage < -255)
voltage = (int16_t)-250;
break;
default:
print_usb( "Command not recognized. Try {UJS}\r\n", 32 );
print_usb( MENU, MENU_LENGTH);
return;
}
length = sprintf( tempBuffer, "Op:%c V:%d\r\n", op_cmd, voltage);
print_usb( tempBuffer, length );
#endif
#ifdef LAB_MODE
//length = sprintf( tempBuffer, "Op:%c V:%f\n\r", op_cmd, op_value );
//print_usb( tempBuffer, length );
if (strlen(buffer) >= 2)
length = sscanf(buffer, "%c %d\n", &op_cmd, &temp_goal);
else
length = sscanf(buffer, "%c", &op_cmd);
op_value = (float)temp_goal;
//G_green_period = 10;
op_cmd -= 0;
switch (op_cmd) {
//Toggle Logging
case 'l':
case 'L':
prepare_to_start_log = log_values_now ? false : true; //prepare to start logging if we weren't already logging, otherwise we must stop logging early
if (log_values_now)
{
log_values_now = false; //we're done logging
log_stop_timestep = curr_timestep;
length = sprintf( tempBuffer, "We're done logging values.\n\r");
print_usb( tempBuffer, length );
ready_to_dump_log = true;
}
else
{
ready_to_dump_log = false;
length = sprintf( tempBuffer, "Use the R/r or S/s command to begin logging.\n\r");
print_usb( tempBuffer, length );
}
break;
//Trigger display
case 'V':
display_values_flag = true;
break;
case 'v':
display_values_flag = true;
break;
//Set reference position (in Rotations)
case 'R':
case 'r':
if (!TARGET_ROT) //if we weren't doing position
loadPositionGains(); //then reset position constants (don't refresh between every command to make experiments easier)
TARGET_ROT = true;
TARGET_RPM = false;
resetValues();
if (USE_DEGREES)
goal = (double)(op_value/360.0);
else
goal = op_value;
//if (!running_Trajectory)
// log_start_timestep = curr_timestep;
just_started_maneuver = true;
if (prepare_to_start_log)
{
length = sprintf( tempBuffer, "Beginning to log position test...\n\r");
print_usb( tempBuffer, length );
prepare_to_start_log = false; //preparation is done; Ready, Set, GO!!!
log_values_now = true;
log_start_timestep = curr_timestep;
}
break;
//Set reference speed (in RPM)
case 'S':
case 's':
if (!TARGET_RPM) //if we weren't doing speed
loadSpeedGains(); //then reset speed constants (don't refresh between every command to make experiments easier)
TARGET_ROT = false;
TARGET_RPM = true;
resetValues();
if (USE_DEGREES)
goal = (double)(op_value/360.0);
else
goal = op_value;
//log_start_timestep = curr_timestep;
just_started_maneuver = true;
if (prepare_to_start_log)
{
length = sprintf( tempBuffer, "Beginning to log speed test...\n\r");
print_usb( tempBuffer, length );
prepare_to_start_log = false; //preparation is done; Ready, Set, GO!!!
log_values_now = true;
log_start_timestep = curr_timestep;
}
break;
//Adjusting Kp
case 'P':
Kp += 0.1;
break;
case 'p':
Kp -= 0.1;
break;
//Adjusting Ki
case 'I':
Ki += 1.0;
break;
case 'i':
Ki -= 1.0;
break;
//Adjusting Kd
case 'D':
Kd += 0.01;
break;
case 'd':
Kd -= 0.01;
break;
case 'B':
case 'b':
Kp = op_value;
break;
case 'N':
case 'n':
Ki = op_value;
break;
case 'M':
case 'm':
Kd = op_value;
break;
case 'T':
case 't':
execute_Trajectory();
break;
//Execute trajectory
default:
//print_usb("Command not recognized. Try again\n\r", 35 );
break;
//print_usb( MENU, MENU_LENGTH);
}
//length = sprintf( tempBuffer, "Op:%c V:%d\r\n", op_dir, op_value );
//print_usb( tempBuffer, length );
#endif
} //end menu()
