static void dump_root(const void *buf)
{
#ifndef __SMALL_TREE__
usb_printf("ROOT\nlroot: %x rroot: %x",
(uint32_t) ((uint64_t *) buf)[0],
(uint32_t) ((uint64_t *) buf)[1]);
#else
usb_printf("ROOT\nroot: %x", (uint32_t) ((uint64_t *) buf)[0]);
#endif // __SMALL_TREE__
}
static void dump_list(const void *buf)
{
uint8_t lsize = (uint8_t) (((uint16_t *) buf)[0] & 0x3F);
usb_printf("LIST %d\n", (((uint16_t *) buf)[0] >> 6));
usb_printf("size: %d\n", lsize);
for (int i = 0 ; i < lsize + 1 ; i++)
usb_printf("N%d: %x ", i, (uint32_t)
((uint64_t *) (((uint8_t *) buf) + 2))[i]);
// This is definitely the worst single line of code I have ever written.
}
/// \brief Handler for the `update` command
static inline bool exec_update(const char * const command)
{
// Make sure the command is well-formed
if (command[8] != ' ') {
return false;
}
// Parse size of incoming update packet
update_bytes_pending = atoi(command + 9);
if (update_bytes_pending == 0) {
return false;
}
// Calculate number of pages
num_pages = update_bytes_pending / SPM_PAGESIZE;
if (update_bytes_pending % SPM_PAGESIZE) {
++num_pages;
}
// Abort if the program is too big to fit into memory
if (num_pages > MAX_PAGE_NUM) {
usb_puts(PSTR("Program is too big! Aborting."));
return true;
}
// Warn if the program is approaching the maximum size
uint8_t pages_left = MAX_PAGE_NUM - num_pages;
if (pages_left < 20) {
usb_printf(PSTR("Warning: Only %u pages of program space left!" USB_NEWLINE), pages_left);
}
// Erase temporary application memory
usb_puts(PSTR("Erasing temporary application flash section..."));
wdt_disable();
uint8_t error_code = xboot_app_temp_erase();
if (error_code != XB_SUCCESS) {
usb_printf(PSTR("Error erasing temporary application section: %d" USB_NEWLINE), error_code);
return true;
}
wdt_reenable();
// Switch to update mode
usb_printf(PSTR("Ready to receive %u bytes (%u pages)..." USB_NEWLINE),
update_bytes_pending,
num_pages);
echo_on = false;
update_in_progress = true;
page_buffer_index = 0;
temp_app_addr = 0;
return true;
}
/// \brief Executes an interactive command
/// \details Tries to match the beginning of the supplied string to the registered commands.
/// If a matching command string is found, its handler is invoked.
/// In case no suitable command could be matched, or the executed handler returns a
/// non-zero value, an error message is sent.
/// \param command
/// the full command line as a C-string
static inline void execute_command(const char * const command)
{
bool success = true;
if (strcmp(command, "clear") == 0) {
for (int i=0; i<80; ++i) {
usb_puts(PSTR(""));
}
}
else if (strncmp(command, "fwupdate", 8) == 0) {
success = exec_update(command);
}
else if (strcmp(command, "help") == 0) {
success = exec_help();
}
else if (strcmp(command, "reset") == 0) {
usb_puts(PSTR("Resetting device..."));
reset = true;
}
else {
// Iterate all user-defined commands and try to find a matching one
for (uint8_t i=0; i<user_commands_size; ++i) {
const struct serial_command* user_command = user_commands + i;
if (strncmp_P(command, user_command->cmd_string, strlen_P(user_command->cmd_string)) == 0) {
success = user_command->handler(command);
goto cleanup;
}
}
// No known command matches :-(
usb_printf(PSTR("Unknown command: [%s]" USB_NEWLINE), command);
usb_puts(PSTR("Type `help` for help." USB_NEWLINE));
}
cleanup:
if (!success) {
usb_printf(PSTR("Error executing command: [%s]" USB_NEWLINE), command);
}
// Clear command buffer and reset write pointer
memset(cmd_buffer, '\0', sizeof(cmd_buffer));
cmd_buffer_index = 0;
}
/**
* @brief Displays the message on LCD on device reset event
* @param speed : device speed
* @retval None
*/
void USBD_USR_DeviceReset (uint8_t speed)
{
/* 在USB未连接状态，这个函数每隔20ms进来一次 */
switch (speed)
{
case USB_OTG_SPEED_HIGH:
usb_printf(" USBD_USR_DeviceReset [HS]\r\n" );
break;
case USB_OTG_SPEED_FULL:
usb_printf(" USBD_USR_DeviceReset [FS]\r\n" );
break;
default:
usb_printf(" USB Device Library v1.1.0 [??]\r\n" );
break;
}
}
//exec time: about 900us
void analog_update_fsm(u08 cmd, u08 *param)
{
static uint8 count=0;
static uint32 vbatt;
uint8 i;
static u08 initialized=0;
//task_open();
if(!initialized)
{
initialized = 1;
usb_printf("analog_update_fsm()\n");
//for(i=0;i<=7;i++) s.inputs.analog[i] = (analog_read(i))>>2;
s.inputs.vbatt = read_battery_millivolts_svp();
vbatt = read_battery_millivolts_svp();
}
//while(1)
{
count++;
//for(i=0;i<=7;i++) s.inputs.analog[i] = (uint8)((((uint16)(s.inputs.analog[i]))*1 + (uint16)(analog_read(i)>>2) )/2);
for(i=0; i<=15; i++)
{
set_digital_output(ANALOG_MUX_ADDR_0_PIN, i & 0x01);
set_digital_output(ANALOG_MUX_ADDR_1_PIN, i & 0x02);
set_digital_output(ANALOG_MUX_ADDR_2_PIN, i & 0x04);
set_digital_output(ANALOG_MUX_ADDR_3_PIN, i & 0x08);
s.inputs.analog[i] = (uint16)(analog_read(4)>>2);
}
s.line[RIGHT_LINE] = s.inputs.analog[AI_LINE_RIGHT];
s.line[LEFT_LINE] = s.inputs.analog[AI_LINE_LEFT];
//sample the following only once every 5 * 20 == 100ms
if( count >= 5)
{
count = 0;
vbatt = (vbatt*7UL + (uint32)read_battery_millivolts_svp())/8UL;
s.inputs.vbatt = (uint16)vbatt;
}
//task_wait(interval);
}
//task_close();
}
static void dump_node(const void *buf)
{
usb_printf("NODE %d\n"
"sons: %x %x %x %x %x %x %x %x",
((uint8_t *) buf)[2] & 0x0F,
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[1],
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[2],
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[3],
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[4],
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[5],
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[6],
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[7],
(uint32_t) ((uint64_t *) ((uint8_t *) buf + 3))[8]);
}
/// \brief Processes a command character
/// \details If the supplied character is a carriage return, the command line read so far is
/// executed, otherwise the character is simply appended to a (circular!) buffer.
/// If #echo_on is set, the character is also immediately sent back to the sender,
/// whereby carriage return characters are replaced by #USB_NEWLINE.
static inline void process_command_char(void)
{
// Fetch a character from the USB data buffer
char data = usb_getc();
// Handle escape sequence if any and abort
if (handle_escape_sequence(data)) {
return;
}
// Echo back the received character if desired and possible
if (echo_on) {
if (data == '\r') {
usb_puts(PSTR(""));
}
else if (0 <= data && data < 128) {
usb_putc(data);
}
}
// Parse and execute commands if the enter key was hit
if (data == '\r') {
// Ignore empty command
if (strcmp(cmd_buffer, "") == 0) {
return;
}
strncpy(cmd_history[cmd_history_index], cmd_buffer, CMD_BUFFER_SIZE);
++cmd_history_index;
cmd_history_index %= CMD_HISTORY_SIZE;
execute_command(cmd_buffer);
return;
}
// Clear last character and rewind buffer index if backspace was received
if (data == '\b') {
usb_printf(PSTR(" \b"));
if (cmd_buffer_index > 0) {
--cmd_buffer_index;
}
cmd_buffer[cmd_buffer_index] = '\0';
return;
}
// Add char to command buffer; cycle on overflow
cmd_buffer[cmd_buffer_index] = data;
++cmd_buffer_index;
cmd_buffer_index %= CMD_BUFFER_SIZE;
}
/// \brief Prints out a command from the history
/// \details Clears out the last printed command first.
/// \param offset
/// offset of the command to be printed
static inline void print_command_from_history(const int8_t offset)
{
// Cycle command history index
cmd_history_index += CMD_HISTORY_SIZE;
cmd_history_index += offset;
cmd_history_index %= CMD_HISTORY_SIZE;
// Overwrite previously printed command
static uint8_t last_cmd_length = 0;
for (int i=0; i < last_cmd_length; ++i) {
usb_printf(PSTR("\b \b"));
cmd_buffer[cmd_buffer_index] = '\0';
}
// Copy command from history to current command buffer
strncpy(cmd_buffer, cmd_history[cmd_history_index], CMD_BUFFER_SIZE);
cmd_buffer_index = strlen(cmd_buffer) % CMD_BUFFER_SIZE;
// Print out command and save command length for the next invocation
usb_printf_S(cmd_buffer);
last_cmd_length = cmd_buffer_index;
}
void test_task(u08 cmd, u08 *param)
{
static u16 i;
float time_to_stop=0,distance_to_stop;
static t_scan_result scan_result;
DEFINE_CFG2(s16,accel, 99,1);
DEFINE_CFG2(s16,decel, 99,2);
DEFINE_CFG2(s16,speed, 99,3);
DEFINE_CFG2(s16,distance, 99,4);
DEFINE_CFG2(s16,angle, 99,5);
task_open_1();
//code between _1() and _2() will get executed every time the scheduler resumes this task
if(cmd==0)
{
NOP();
}
else
{
NOP();
return;
}
task_open_2();
//execution below this point will resume wherever it left off when a context switch happens
usb_printf("test_task()\n");
PREPARE_CFG2(accel);
PREPARE_CFG2(decel);
PREPARE_CFG2(speed);
PREPARE_CFG2(distance);
PREPARE_CFG2(angle);
test_task(1,(uint8 *)0x1234);
/*
for(;;)
{
dbg_printf("0123456789\n");
task_wait(100);
}
*/
/*
task_wait(200);
motor_command(2,0,0,0,0);
task_wait(200);
motor_command(7,0,0,100,100);
task_wait(500);
motor_command(6,2,2,0,0);
task_wait(500);
*/
while(1)
{
task_wait(100);
UPDATE_CFG2(accel);
UPDATE_CFG2(decel);
UPDATE_CFG2(speed);
UPDATE_CFG2(distance);
UPDATE_CFG2(angle);
if(s.behavior_state[TEST_LOGIC_FSM]==1)
{
/*
1) gradually ramp up (at specified rate) to target speed
2) when we are <= 30degrees from the target, start ramping down to speed 15
3) when we are <= 10degrees from the target, apply target speed 5 (w/ feed forward) & regulate to maintain 5
3) when we are at the target, hit the brakes - full stop w/out ramping down
*/
/*
odometry_set_checkpoint();
motor_command(6,1,1,(speed),-(speed));
while ( abs(odometry_get_rotation_since_checkpoint()) < 60 ) { task_wait(10); }
motor_command(6,1,1,15,-15);
while ( abs(odometry_get_rotation_since_checkpoint()) < 80 ) { task_wait(10); }
motor_command(7,1,1,5,-5);
while ( abs(odometry_get_rotation_since_checkpoint()) < 90 ) { task_wait(10); }
motor_command(7,1,1,0,0);
*/
/*
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
*/
#if 0
//set_digital_output(IO_D1,0);
set_digital_output(IO_D0,0);
task_wait(2000);
//set_digital_output(IO_D1,1);
set_digital_output(IO_D0,1);
#endif
#if 0
//dbg_printf("Starting scan....\n");
TURN_IN_PLACE_AND_SCAN( 40, 220 );
//dbg_printf("....done\n");
scan_result = find_peak_in_scan(scan_data,360,30);
dbg_printf("scan_result: %d,%d,%d,%d,%d,%d\n",
scan_result.flame_center_value, scan_result.rising_edge_position, scan_result.falling_edge_position,
scan_result.center_angle, scan_result.rising_edge_angle, scan_result.falling_edge_angle);
for(i=0;i<360;i++) {dbg_printf("scan_data[%03d]=%03d,%03d\n",i, scan_data[i].angle, scan_data[i].flame);task_wait(10);}
TURN_IN_PLACE( 40, -(220-scan_result.center_angle) );
#endif
TURN_IN_PLACE_AND_SCAN( 40, 90, 1);
scan_result = find_path_in_scan(scan_data, 100, 300, 0, 1);
dbg_printf("scan_result: %d,%d,%d,%d\n", scan_result.opening, scan_result.center_angle, scan_result.rising_edge_angle, scan_result.falling_edge_angle);
//for(i=0;i<100;i++) {dbg_printf("scan_data[%03d]=%03d,%03d\n",i, scan_data[i].angle, scan_data[i].ir_north);task_wait(10);}
TURN_IN_PLACE(40,-90);
/*
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, -90 );
scan_result = find_peak_in_scan(scan_data,360,3);
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, 180 );
scan_result = find_peak_in_scan(scan_data,360,3);
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, -180 );
scan_result = find_peak_in_scan(scan_data,360,3);
*/
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==2)
{
TURN_IN_PLACE(speed,angle);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==3)
{
TURN_IN_PLACE(40, -90);
TURN_IN_PLACE_AND_SCAN(40, 180, 4);
scan_result = find_flame_in_scan(scan_data,360,30);
if(scan_result.flame_center_value > 150) //TODO: make the minimum flame value a parameter
{
static int i, i_min;
static u16 min=999;
static float d;
static u08 stop=0;
TURN_IN_PLACE( 40, -(180-scan_result.center_angle+2) );
/*
min=999;
for(i=scan_result.rising_edge_position-10; i<=scan_result.falling_edge_position+10; i++)
{
dbg_printf("scan_data[%3d]: ir_n=%3d, a=%d, f=%d\n",i,scan_data[i].ir_north, scan_data[i].angle, scan_data[i].flame);
task_wait(50);
if(scan_data[i].ir_north < min) { min=scan_data[i].ir_north; i_min=i; }
}
dbg_printf("distance to candle: %d @i=%d,a=%d\n",min,i_min,scan_data[i_min].angle);
if(min<100) min=100;
d = (float) (((min-100)*25)/10);
MOVE2(50,d,60,60);
*/
stop=0;
move_manneuver2(1,30,9999,(80),(90));
while(move_manneuver2(0,30,9999,(70),(70)))
{
OS_SCHEDULE;
if( (s.ir[IR_N] <= 60) ) stop |= 0x01;
if( (s.ir[IR_NE] <= 60)) stop |= 0x02;
if( (s.ir[IR_NW] <= 60)) stop |= 0x04;
if( (s.inputs.sonar[0] <= 100) ) stop |= 0x08;
if(stop != 0)
{
dbg_printf("too close to object/wall! reason: 0x%02x\n",stop);
HARD_STOP();
break;
}
}
}
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==4)
{
PUMP_ON();
task_wait(1000);
PUMP_OFF();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==5)
{
dbg_printf("start = %d\n",is_digital_input_high(IO_B3));
task_wait(500);
}
/*
while(s.behavior_state[11]==1)
{
time_to_stop = (float)s.inputs.actual_speed[0] / (float)50;
distance_to_stop = ((float)s.inputs.actual_speed[0] * time_to_stop)/2.0;
distance_to_stop *= 50.0; //adjust for seconds
distance_to_stop *= 0.13466716824940938560464; //adjust for mm
if(s.inputs.x + distance_to_stop < distance)
{
motor_command(6,accel,decel,speed,speed);
}
else
{
motor_command(6,accel,decel,0,0);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
task_wait(20);
}
*/
}
task_close();
}
/**
* @brief Displays the message on LCD on device lib initialization
* @param None
* @retval None
*/
void USBD_USR_Init(void)
{
usb_printf("> USBD_USR_Init \r\n" );
}
static void dump_message(const void *buf)
{
usb_printf("MESSAGE\n%s", buf);
}
/**
* @brief Displays the message on LCD on device suspend event
* @param None
* @retval None
*/
void USBD_USR_DeviceSuspended(void)
{
usb_printf("> Device In suspend mode.\r\n");
}
/**
* @brief Displays the message on LCD on device config event
* @param None
* @retval Staus
*/
void USBD_USR_DeviceConfigured (void)
{
usb_printf("> MSC Interface started.\r\n");
}
void vcom_service_input(struct vcom * vcom, uint8_t buf[], int len)
{
usb_cdc_class_t * cdc = vcom->cdc;
struct serial_config cfg;
int i;
int c;
if (vcom->mode == VCOM_MODE_SERVICE) {
usb_printf(cdc, "\r\n\r\n");
usb_printf(cdc, "--- U2S-485 %d.%d -------------\r\n\r\n",
FW_VERSION_MAJOR, FW_VERSION_MINOR);
usb_printf(cdc, "--- Service mode ...\r\n\r\n");
vcom->mode = VCOM_MODE_INTERACTIVE;
}
for (i = 0; i < len; ++i) {
c = buf[i];
(void)c;
switch (c) {
case 'q':
usb_printf(cdc, " - Converter mode...\r\n");
vcom->mode = VCOM_MODE_CONVERTER;
break;
case 'T':
usb_printf(cdc, " - SDU trace mode...\r\n");
vcom->mode = VCOM_MODE_SDU_TRACE;
sdu_trace_init(cdc);
break;
case 't':
usb_printf(cdc, " - Interactive mode...\r\n");
vcom->mode = VCOM_MODE_INTERACTIVE;
break;
case 'F':
if (vcom->mode == VCOM_MODE_INTERACTIVE) {
usb_printf(cdc, " - Firmware update...\r\n");
usb_xflash(0, 32 * 1024);
}
break;
case '1':
cfg.baudrate = 9600;
cfg.databits = 8;
cfg.parity = 0;
cfg.stopbits = 1;
serial_rx_disable(vcom->serial);
serial_config_set(vcom->serial, &cfg);
serial_rx_enable(vcom->serial);
usb_printf(cdc, " - 9600 8N1\r\n");
break;
case '2':
cfg.baudrate = 19200;
cfg.databits = 8;
cfg.parity = 0;
cfg.stopbits = 1;
serial_rx_disable(vcom->serial);
serial_config_set(vcom->serial, &cfg);
serial_rx_enable(vcom->serial);
usb_printf(cdc, " - 19200 8N1\r\n");
break;
case '3':
cfg.baudrate = 38400;
cfg.databits = 8;
cfg.parity = 0;
cfg.stopbits = 1;
serial_rx_disable(vcom->serial);
serial_config_set(vcom->serial, &cfg);
serial_rx_enable(vcom->serial);
usb_printf(cdc, " - 38400 8N1\r\n");
break;
case '4':
cfg.baudrate = 57600;
cfg.databits = 8;
cfg.parity = 0;
cfg.stopbits = 1;
serial_rx_disable(vcom->serial);
serial_config_set(vcom->serial, &cfg);
serial_rx_enable(vcom->serial);
usb_printf(cdc, " - 57600 8N1\r\n");
break;
case '5':
cfg.baudrate = 115200;
cfg.databits = 8;
cfg.parity = 0;
cfg.stopbits = 1;
serial_rx_disable(vcom->serial);
serial_config_set(vcom->serial, &cfg);
serial_rx_enable(vcom->serial);
usb_printf(cdc, " - 115200 8N1\r\n");
break;
case 'U':
sdu_trace_show_supv(true);
usb_printf(cdc, " - Show supervisory...\r\n");
break;
case 'u':
sdu_trace_show_supv(false);
usb_printf(cdc, " - Don't show supervisory...\r\n");
break;
case 'P':
sdu_trace_show_pkt(true);
usb_printf(cdc, " - Show packets...\r\n");
break;
case 'p':
sdu_trace_show_pkt(false);
usb_printf(cdc, " - Don't show packets...\r\n");
break;
case 'A':
sdu_trace_time_abs(true);
usb_printf(cdc, " - Absolute timestamps...\r\n");
break;
case 'a':
sdu_trace_time_abs(false);
usb_printf(cdc, " - Relative timestamps...\r\n");
break;
case 'v':
usb_printf(cdc, "\r\n");
usb_printf(cdc,
"0123456789abcdef0123456789ABCDEF0123456789abcdef0123456789ABCDEF"
"0123456789abcdef0123456789ABCDEF0123456789abcdef0123456789ABCDEF");
usb_printf(cdc, "\r\n");
usb_printf(cdc,
"0123456789abcdef0123456789ABCDEF0123456789abcdef0123456789ABCDEF"
"0123456789abcdef0123456789ABCDEF0123456789abcdef0123456789ABCDEF");
usb_printf(cdc, "\r\n");
usb_printf(cdc,
"0123456789abcdef0123456789ABCDEF0123456789abcdef0123456789ABCDEF"
"0123456789abcdef0123456789ABCDEF0123456789abcdef0123456789ABCDEF");
/*
usb_printf(cdc, "\r\n"
"1. The qick brown fox jumps over the lazy dog.\r\n"
"2. THE QICK BROWN FOX JUMPS OVER THE LAZY DOG.\r\n");
usb_printf(cdc,
"3. The qick brown fox jumps over the lazy dog.\r\n"
"4. THE QICK BROWN FOX JUMPS OVER THE LAZY DOG.\r\n");
usb_printf(cdc,
"5. The qick brown fox jumps over the lazy dog.\r\n");
usb_printf(cdc,
"6. THE QICK BROWN FOX JUMPS OVER THE LAZY DOG.\r\n");
*/
break;
default:
show_menu(cdc);
}
}
}
/**
* @brief Displays the message on LCD on device resume event
* @param None
* @retval None
*/
void USBD_USR_DeviceResumed(void)
{
usb_printf("> Device Resumed.\r\n");
}
/// \brief Handler for the `help` command
static inline bool exec_help(void)
{
usb_puts(PSTR(""));
usb_puts(PSTR("Welcome to the uMIDI serial interface!"));
usb_printf(PSTR("Software ID: %s" USB_NEWLINE), UMIDI_SOFTWARE_ID);
usb_puts(PSTR("Built-in commands:"));
usb_puts(PSTR(" clear : Clears the console by printing CR/LFs."));
usb_puts(PSTR(" fwupdate <s> : Initiates a firmware update:"));
usb_puts(PSTR(" <s> : firmware update packet size"));
usb_puts(PSTR(" help : Prints this help message."));
usb_puts(PSTR(" reset : Resets the device."));
if (user_commands_size) {
usb_puts(PSTR("Special commands:"));
}
for (uint8_t i=0; i<user_commands_size; ++i) {
// Copy strings to RAM for processing
uint16_t cmd_string_size = strlen_P(user_commands[i].cmd_string)+1;
char* cmd_string = malloc(cmd_string_size);
strlcpy_P(cmd_string, user_commands[i].cmd_string, cmd_string_size);
uint16_t help_string_size = strlen_P(user_commands[i].help_string)+1;
char* help_string = malloc(help_string_size);
strlcpy_P(help_string, user_commands[i].help_string, help_string_size);
// Check if the help string contains newline characters
const char* first_nl = strchr(help_string, '\n');
if (first_nl) {
// Parse specified options / parameters to the command string
char* params = strtok(help_string, "\n");
if (help_string == first_nl) {
// If the first character of the help string is a newline character, the parsed
// params are actually the first line of the description text.
usb_printf(PSTR(" %-16s : %s" USB_NEWLINE), cmd_string, params);
}
else {
// Append options / parameters to command string and print the whole thing
char first_column[17] = "";
snprintf(first_column, sizeof(first_column),
"%s %s", cmd_string, params);
usb_printf(PSTR(" %-16s : "), first_column);
// Complete the first line of the help string
usb_printf(PSTR("%s" USB_NEWLINE), strtok(NULL, "\n"));
}
// Split remaining command description at '\n' chars, pad with spaces and print
char* tail = strtok(NULL, "\n");
while (tail) {
usb_printf(PSTR(" %s" USB_NEWLINE), tail);
tail = strtok(NULL, "\n");
}
}
else {
// Print simple command description
usb_printf(PSTR(" %-16s : %s" USB_NEWLINE), cmd_string, help_string);
}
free(help_string);
}
usb_puts(PSTR("Please enter a command:"));
// Success
return true;
}
/**
* @brief USBD_USR_DeviceConnected
* Displays the message on LCD on device connection Event
* @param None
* @retval Staus
*/
void USBD_USR_DeviceConnected (void)
{
usb_printf("> USB Device Connected.\r\n");
//g_ucCommRun = 1;
}
/**
* @brief USBD_USR_DeviceDisonnected
* Displays the message on LCD on device disconnection Event
* @param None
* @retval Staus
*/
void USBD_USR_DeviceDisconnected (void)
{
usb_printf("> USB Device Disconnected.\r\n");
}
void test_fsm(u08 cmd, u08 *param)
{
//enum states { s_disabled=0, s_tracking_wall=1, s_lost_wall=2, s_turning_corner=3, s_turning_sharp_corner=4 };
static u08 state=0;
static u08 last_state=255;
static u32 t_start;
static s16 bias=0;
static u08 count;
u32 t_delta;
u08 i;
t_config_value v;
static u08 initialized=0;
if(!initialized)
{
initialized=1;
usb_printf("wall_follow_fsm()\n");
}
if(s.behavior_state[TEST_LOGIC_FSM]==1)
{
PUMP_ON();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==2)
{
PUMP_OFF();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==3)
{
dbg_printf("start = %d\n",is_digital_input_high(IO_B3));
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==4)
{
switch(state)
{
case 0:
t_start = get_ms();
motor_command(8,0,0,speed_profile[0].l_speed,speed_profile[0].r_speed);
state++;
break;
case 1:
t_delta = get_ms() - t_start;
i=0;
while(speed_profile[i].time < t_delta) i++;
motor_command(8,0,0,speed_profile[i-1].l_speed,speed_profile[i-1].r_speed);
break;
}
}
if(s.behavior_state[TEST_LOGIC_FSM]==5)
{
static s16 ne=0, nw=0;
ne = (ne + (s16) s.inputs.analog[AI_FLAME_NE])/2;
nw = (nw + (s16) s.inputs.analog[AI_FLAME_NW])/2;
bias = 0;
if( (ne>245) && (nw>245) ) bias = 0;
else if( abs(ne-nw) < 10 ) bias = 0;
else if( ne>nw ) bias = -1;
else if( nw>ne ) bias = 1;
v.u16 = cfg_get_u16_by_grp_id(15,6);
v.u16 += bias;
cfg_set_value_by_grp_id(15,6, v);
}
if(s.behavior_state[TEST_LOGIC_FSM]==6)
{
if(s.inputs.analog[AI_FLAME_NW]<80)
{
motor_command(6,3,3,-40,40);
}
else
{
motor_command(2,0,0,0,0);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
}
}
void ultrasonic_update_fsm(uint8 cmd, uint8 *param)
{
enum states
{
s_waiting_for_ping=0, //0
s_waiting_for_echo, //1
s_none=255
};
static enum states state=s_waiting_for_ping;
static enum states last_state=s_none;
static u32 t_entry=0;
static u08 initialized=0;
static u08 newpulse=0;
static u32 t_ping=0;
static u32 pulse;
static u16 distance;
static u08 sensor=0;
DEFINE_CFG2(u08,bitmap,4,1);
DEFINE_CFG2(u32,echo_timeout,4,2);
DEFINE_CFG2(u32,intra_delay,4,3);
//task_open();
if(!initialized)
{
initialized=1;
usb_printf("ultrasonic_update_fsm()\n");
PREPARE_CFG2(bitmap);
PREPARE_CFG2(echo_timeout);
PREPARE_CFG2(intra_delay);
}
//while(1)
{
UPDATE_CFG2(bitmap);
UPDATE_CFG2(echo_timeout);
UPDATE_CFG2(intra_delay);
first_(s_waiting_for_ping)
{
enter_(s_waiting_for_ping)
{
NOP();
}
if(get_ms() - t_ping >= intra_delay)
{
PING(sonar_pin[sensor]);
newpulse = new_pulse(sensor); //clear the pulse capture state
t_ping = get_ms();
state = s_waiting_for_echo;
}
exit_(s_waiting_for_ping)
{
NOP();
}
}
next_(s_waiting_for_echo)
{
enter_(s_waiting_for_echo)
{
NOP();
}
newpulse = new_high_pulse(sensor);
if(newpulse)
{
pulse = pulse_to_microseconds(get_last_high_pulse(sensor));
distance = ((pulse*10)/148) + 2;
state = s_waiting_for_ping;
}
else if( get_ms()-t_ping >= echo_timeout)
{
distance = 4000;
state = s_waiting_for_ping;
}
exit_(s_waiting_for_echo)
{
s.inputs.sonar[sensor] = distance;
s.us_avg[sensor] = (s.us_avg[sensor]*3 + distance)/4;
sensor=get_next_sensor();
if(get_ms() - t_ping >= intra_delay)
{
PING(sonar_pin[sensor]);
newpulse = new_pulse(sensor);
t_ping = get_ms();
state = s_waiting_for_echo;
}
}
}
}
}
/// \brief Processes binary data arriving over USB during the update process
/// \details If the supplied character is a carriage return, the command line read so far is
/// executed, otherwise the character is simply appended to a (circular!) buffer.
static inline void process_update_data(void)
{
// Receive and copy bytes to page buffer
while (usb_bytes_received()) {
page_buffer[page_buffer_index] = usb_getc();
++page_buffer_index;
--update_bytes_pending;
}
// When the last byte has arrived
if (update_bytes_pending == 0) {
// Extract expected CRC checksum
expected_crc = page_buffer[page_buffer_index-2] << 8;
expected_crc |= page_buffer[page_buffer_index-1];
if (expected_crc == 0) {
usb_puts(PSTR("Invalid empty CRC found!"));
return;
}
usb_printf(PSTR("Expected CRC checksum: %x" USB_NEWLINE), expected_crc);
// Rewind page_buffer_index
page_buffer_index -= 2;
// Pad last page with 0xFF for CRC calculation
usb_puts(PSTR("Padding last application page..."));
while (page_buffer_index < SPM_PAGESIZE)
{
page_buffer[page_buffer_index] = 0xff;
++page_buffer_index;
}
}
// When a page is full, write it to the temporary application flash
if (page_buffer_index == SPM_PAGESIZE) {
// TODO: For unknown reasons, the following call prints 0 for the total number of pages:
/*
usb_printf(PSTR("Writing temporary application page [%3u/%3u]..." USB_NEWLINE),
temp_app_addr / SPM_PAGESIZE + 1,
num_pages);
*/
usb_printf(PSTR("Writing temporary application page [%3u"), temp_app_addr / SPM_PAGESIZE + 1);
usb_printf(PSTR("/%3u/%3u]..." USB_NEWLINE), num_pages, MAX_PAGE_NUM);
if (xboot_app_temp_write_page(temp_app_addr, page_buffer, 0) != XB_SUCCESS) {
goto fail;
}
temp_app_addr += SPM_PAGESIZE;
page_buffer_index = 0;
}
if (update_bytes_pending == 0) {
// Calculate CRC of the written application and compare with expected CRC
usb_puts(PSTR("Performing CRC..."));
uint16_t actual_crc = 0;
xboot_app_temp_crc16(&actual_crc);
if (expected_crc != actual_crc) {
usb_printf(PSTR("CRC checksum mismatch: %x != %x" USB_NEWLINE), expected_crc, actual_crc);
goto fail;
}
usb_puts(PSTR("Success!"));
// Tell xboot to install the firmware on next reset
if (xboot_install_firmware(expected_crc) != XB_SUCCESS) {
goto fail;
}
// Reset device
usb_puts(PSTR("Resetting device and installing new firmware..."));
reset = true;
}
return;
fail:
// Return to normal operation if the update failed
usb_puts(PSTR("Update failed!"));
update_in_progress = false;
echo_on = true;
}
void show_menu(usb_cdc_class_t * cdc)
{
usb_printf(cdc, "--- Option:\r\n");
usb_printf(cdc, " [1] 9600 8N1\r\n");
usb_printf(cdc, " [2] 19200 8N1\r\n");
usb_printf(cdc, " [3] 38400 8N1\r\n");
usb_printf(cdc, " [4] 57600 8N1\r\n");
usb_printf(cdc, " [5] 115200 8N1\r\n");
usb_printf(cdc, " [q] quit\r\n");
usb_printf(cdc, " [F] firmware update\r\n");
usb_printf(cdc, " [T/t] enable/disable trace\r\n");
usb_printf(cdc, " [A/a] absolute/relative time\r\n");
usb_printf(cdc, " [U/u] enable/disable supervisory\r\n");
usb_printf(cdc, " [P/p] enable/disable packets\r\n");
};
int main(void) {
volatile int16_t* samples;
unsigned int i;
DISABLE_GLOBAL_INT();
/* stop watchdog timer */
WDTCTL = WDTPW +WDTHOLD;
/* SET CPU to 5MHz */
/* max DCO
MCLK = DCOCLK
SMCLK = DCOCLK
ACLK = 8KHz
*/
DCOCTL = DCO0 + DCO1 + DCO2;
BCSCTL1 = RSEL0 + RSEL1 + RSEL2 + XT2OFF;
BCSCTL2 = 0x00;
delay_us(10000);
/* activate Active Mode */
__bic_SR_register(LPM4_bits);
/* set LEDs when loaded */
P5SEL = 0x00;
P5DIR = 0x70;
LED_RED_ON();
LED_GREEN_OFF();
LED_BLUE_OFF();
check_for_clock();
init_usb_serial();
#ifdef USE_DMA
init_dma(&g_sample_flag);
#endif
#ifdef TX
init_adc(&g_sample_flag);
#else
init_dac();
#endif
init_rf(RF_CHANNEL, PAN_ID, NODE_ADDR, &g_sample_flag);
debug_print("Successfully booted.\n");
/* set LEDS to signalize finished initilizing */
LED_RED_OFF();
ENABLE_GLOBAL_INT();
#ifdef TX
/* TX */
while(1) {
if(g_sample_flag == 1) {
g_sample_flag = 0;
#ifdef USE_DMA
/* get samples */
samples = get_samples_dma();
#else
/* get samples */
samples = get_samples();
#endif
/* send oder radio, 2*num_words */
send_rf_data(RF_RX_ADDR, (uint8_t*) samples, NUM_SAMPLES*2);
}
/* reset WDT */
WDTCTL = WDTPW + WDTCNTCL;
}
#else
/* RX */
while(1) {
if(g_sample_flag == 1) {
g_sample_flag = 0;
samples = get_samples_rf();
#if 0
uint8_t err = 0;
for(i = 0; i < NUM_SAMPLES; ++i) {
//samples[i] = 4095-7*i;
usb_printf("%d\n", samples[i]);
//if( ((uint16_t) samples[i]) > 4095) {
// usb_printf("i=%u\n", i);
// ++err;
//}
}
usb_printf("#error: %u\n", err);
usb_printf("\n\n");
#endif
set_dma_data(samples, NUM_SAMPLES);
}
/* reset WDT */
WDTCTL = WDTPW + WDTCNTCL;
}
#endif
return 0;
}
//volatile int busyflag=0;
//volatile char _samserial_buffer[128];
void samserial_print(const char* c)
{
usb_printf(c);
}
