Commit b0aa6b1e authored by Daniel Gröber's avatar Daniel Gröber
Browse files

Release v5.1

parent 85d4d2ed
......@@ -3,7 +3,9 @@ Robo command reference
Communication with the robot is done using a 3V3 or 5V serial (UART) connection
at 115200 baud with no parity and one stop bit (default settings for most serial
terminal programs)
terminal programs). The header (P2) has 4 pins (3v3, Rx, Tx, GND) you should not
connect 3v3 to the 5V USB-UART interface we're using as this could cause damage
to your computer or the robot.
On a GNU/Linux system you can connect to the serial console using a terminal
program like minicom but I find these quite cumbersome and recommend 'picocom'
......@@ -16,22 +18,16 @@ holding the Ctrl key while pressing and releasing the "A" key followed by the
"X" key, Ctrl can be let go of in between, doesn't matter. For screen it is
`C-a C-d`
On power-on the robot will send some debug info most of which should be ignored:
On power-on the robot send the following string identifying the software version
hello world
rcc
adc calibrate
adc setup
adc chans: xxxx
gpio setup
adc enable
robo v5
The only interesting bit is "robo v5", this is the combined software and
hardware version of the robot. After this line is sent the robot will wait for
commands. The current hardware revision is v5 and the current software version
is omitted in the first release. Future software releases on v5 hardware will be
numbered v5.1, v5.2 and so on.
robo v5.1
The only interesting bit is "robo vX.Y", this is the combined hardware (X) and
software (Y) version of the robot. After this line is sent the robot will wait
for commands. The current hardware revision is "5" and the current software
version is "1" . Future software releases on v5 hardware will be numbered v5.2,
v5.3 and so on.
The command format is stack based, the robot expects zero or more arguments to
be pushed onto the stack followed by a command code. The command's arguments
......@@ -41,15 +37,15 @@ on the stack an error is returned.
The command interface of the robot is designed to be human operable for easy
exploration. This means the robot will immediately echo any character sent (so a
serial terminal program can be used without additional setup) and commands sent
to the robot are terminated by an (optional) CR (ASCII 0x0d) followed by LF
(ASCII 0x0a) we call this the EOL (end of line) sequence. When responding to
commands (other than the inherent echo) CR followed by LF is always used to
denote the EOL.
to the robot are terminated by CR (ASCII 0x0d) optionally followed by LF (ASCII
0x0a) we call this the EOL (end of line) sequence. When responding to commands
(other than the inherent echo) CR followed by LF is always used to denote the
EOL.
Arguments are formatted as signed hexadecimal numbers without a "0x" prefix,
i.e. /-?[0-9]{1,4}/. Examples: the hex numbers (0, 1234, bfff, -cafe, -80)
represent decimal numbers (0, 4660, 49151, -51966, -128) respectively. Commands
are simply single letter characters.
i.e. /-?[0-9]{1,4}/ (regular expression). Examples: the hex numbers (0, 1234,
bfff, -cafe, -80) represent decimal numbers (0, 4660, 49151, -51966, -128)
respectively. Commands are simply single letter lower case characters.
To push an argument onto the stack we send the characters representing it's
value at the beginning of a line followed by the EOL sequence.
......@@ -57,8 +53,10 @@ value at the beginning of a line followed by the EOL sequence.
To empty the stack we send the EOL sequence at the beginning of a line.
Unknown commands, errors and invalid arguments do not alter the stack's state
and are indicated by the robot sending the character "N" followed by the EOL
sequence.
and are indicated by the robot sending the EOL sequence, one of the characters
{"U", "F", "R", "N"} and EOL again. The response codes represent Unknown command
code, Format error (not enough arguments), Range error (argument value out of
range) and generic NACK.
Thus if the robot is in an unknown state sending the EOL sequence twice will
always result in the robot being at the beginning of a line with an empty stack.
......@@ -66,9 +64,19 @@ always result in the robot being at the beginning of a line with an empty stack.
In the following examples ">" denotes host to robot comms (request) and "<"
denotes robot to host comms (response).
Command code reference
Command reference
---
- Push command argument
Request format:
[-0x80000000..0x7fffffff]
Response format:
A
- "i" -- Set motor target velocity
Request format:
......@@ -79,7 +87,7 @@ Command code reference
Response format:
I
A
(Just the letter capital "I" on a line of it's own)
......@@ -95,7 +103,7 @@ Command code reference
<4000
>i
<i
<I
<A
- "s" -- Stop (set left and right motor target velocity to 0)
......@@ -105,7 +113,7 @@ Command code reference
Response format
S
A
- "o" -- Set servo level
......@@ -116,7 +124,15 @@ Command code reference
Response format:
O
A
When set to exactly 0x0, 0xffff or values very close the servo isn't going to
react since it's controlled via the relative time difference of output
transitions generated by pulse width modulation. At these values the
controller output is always off (0x0) or always on (0xffff) so there are no
transitions for the servo to measure.
- "q" -- Query distance sensors
......@@ -132,7 +148,8 @@ Command code reference
>q
<q
<Q fff 468 1285 164
<Q
<fff 468 1285 164
The returned numbers are 12 bit values representing the voltage returned by
the optical distance sensors, call these numbers S1..S4. To calculate the
......@@ -140,23 +157,24 @@ Command code reference
Vx = ( 3.3 / (2^12 - 1) ) * Sx
- "d" -- Query motor encoder deltas
- "e" -- Query motor encoder deltas
Request format:
d
m
Response format:
D [d_systicks] [delta_left] [delta_right]
M [d_systicks] [delta_left] [delta_right]
Example:
>d
<d
<D 16 -123 654
>m
<m
<M
<16 -123 654
This means the last "d" query occurred 16 systicks ago. Since then the left
This means the last "e" query occurred 16 systicks ago. Since then the left
motor moved by -0x123 ticks (i.e. in the reverse direction) and the right
motor moved 0x654 ticks in the forward direction.
......@@ -170,15 +188,13 @@ Command code reference
V [velo_left] [velo_right]
BUG BUG BUG (robo v5): The numbers returned here are in signed decimal
notation instead of hex.
Example:
>v
<v
<V 75 -73
<V
<af -c0
This means the left motor is moving at a rate of 75 encoder ticks per systick
in the forward direction and the right motor is moving at 73 ticks in the
This means the left motor is moving at a rate of 175 encoder ticks per systick
in the forward direction and the right motor is moving at 205 ticks in the
reverse direction.
......@@ -117,7 +117,7 @@ void sc_init(void)
TIM_CR1_DIR_UP);
/* (48e6 / 0x20) / 0x7530 = 50Hz */
timer_set_prescaler(TIM14, 0x20);
timer_set_prescaler(TIM14, 0x10);
timer_set_period(TIM14, SC_PER);
timer_set_oc_value(TIM14, TIM_OC1, 0);
......@@ -491,7 +491,7 @@ void dma1_channel1_isr(void) {
void sens_init(void)
{
u_printf("rcc\r\n");
/* u_printf("rcc\r\n"); */
rcc_periph_clock_enable(RCC_ADC);
rcc_osc_on(RCC_HSI14);
......@@ -499,7 +499,7 @@ void sens_init(void)
rcc_periph_clock_enable(RCC_DMA);
rcc_periph_clock_enable(SENS_RCC_GPIO);
u_printf("adc calibrate\r\n");
/* u_printf("adc calibrate\r\n"); */
adc_set_clk_source(ADC, /* ADC_CFGR2_CKMODE_PCLK_DIV4 */ADC_CLKSOURCE_ADC);
......@@ -510,7 +510,7 @@ void sens_init(void)
adc_calibrate_start(ADC);
adc_calibrate_wait_finish(ADC);
u_printf("adc setup\r\n");
/* u_printf("adc setup\r\n"); */
adc_set_sample_time_on_all_channels(ADC, ADC_SMPTIME_239DOT5);
......@@ -523,19 +523,19 @@ void sens_init(void)
adc_set_single_conversion_mode(ADC);
adc_enable_discontinuous_mode(ADC);
u_printf("adc chans: %x\r\n", ADC_CHSELR(ADC));
/* u_printf("adc chans: %04x\r\n", ADC_CHSELR(ADC)); */
adc_set_resolution(ADC, ADC_RESOLUTION_12BIT);
adc_set_right_aligned(ADC);
u_printf("gpio setup\r\n");
/* u_printf("gpio setup\r\n"); */
gpio_mode_setup(SENS_GPIO,
GPIO_MODE_ANALOG,
GPIO_PUPD_NONE,
SENS_GPIO_PINS);
u_printf("adc enable\r\n");
/* u_printf("adc enable\r\n"); */
adc_enable_eoc_sequence_interrupt(ADC);
adc_enable_eoc_interrupt(ADC);
......@@ -661,6 +661,7 @@ void sys_tick_handler(void)
enum com_state {
COM_EXPECTING_COMMAND = 0xc0300,
COM_EXPECTING_COMMAND_OR_LF,
};
enum com_ack {
......@@ -718,6 +719,7 @@ int com_execute(char *cmd)
if(com.stack_idx < 1)
return -1;
u_printf("\r\nVAL: %x", ((int)SC_PER * com.stack[idx-1]) / UINT16_MAX);
timer_set_oc_value(TIM14, TIM_OC1,
((int)SC_PER * com.stack[idx-1]) / UINT16_MAX);
......@@ -726,11 +728,11 @@ int com_execute(char *cmd)
case 'q': return COM_ACK_SENSORS;
case 'd': return COM_ACK_DELTA;
case 'm': return COM_ACK_DELTA;
case 'v': return COM_ACK_VELOCITY;
default:
if(isdigit((int)c) || c == '-') {
if(isxdigit((int)c) || c == '-') {
char *endptr;
errno = 0;
long int x = strtol(cmd, &endptr, 16);
......@@ -755,12 +757,12 @@ int com_execute(char *cmd)
void com_send_resp(char c)
{
u_printf("%c\r\n", c);
u_printf("\r\n%c\r\n", c);
}
void com_send_sensors(void)
{
u_printf("%x %x %x %x %x\r\n",
u_printf("%04x %04x %04x %04x %04x\r\n",
sens_buffer[0],
sens_buffer[1],
sens_buffer[2],
......@@ -770,7 +772,7 @@ void com_send_sensors(void)
void com_send_delta(void)
{
u_printf("%x %s%x %s%x\r\n",
u_printf("%04x %s%04x %s%04x\r\n",
me_ud_systicks,
me_user_delta[0] < 0 ? "-" : "",
me_user_delta[0] < 0 ? -me_user_delta[0] : me_user_delta[0],
......@@ -785,50 +787,54 @@ void com_send_delta(void)
void com_send_velocity(void)
{
u_printf("%d %d\r\n", me_velocity[0], me_velocity[1]);
u_printf("%04x %04x\r\n", me_velocity[0], me_velocity[1]);
}
void com_loop(void)
{
uint16_t c = usart_recv_blocking(USART2);
/* u_printf("%x ", c); */
/* u_printf("%04x ", c); */
usart_send_blocking(USART2, c);
switch(com.st) {
default:
case COM_EXPECTING_COMMAND:
if(c == '\r')
break;
else if(c == '\n') {
com.command_buf[com.command_idx] = '\0';
com.command_idx = 0;
int rv = com_execute(com.command_buf);
if(rv == COM_ACK) {
com_send_resp('A');
} else if(rv == COM_ACK_SENSORS) {
com_send_resp('Q');
com_send_sensors();
} else if(rv == COM_ACK_DELTA) {
com_send_resp('D');
com_send_delta();
} else if(rv == COM_ACK_VELOCITY) {
com_send_resp('V');
com_send_velocity();
} else if (rv == COM_NACK_UNKNOWN) {
com_send_resp('U');
} else if (rv == COM_NACK_FORMAT) {
com_send_resp('F');
} else if (rv == COM_NACK_RANGE) {
com_send_resp('R');
} else if (rv < 0){
com_send_resp('N');
}
break;
default:
case COM_EXPECTING_COMMAND_OR_LF:
if(c == '\n') {
com.st = COM_EXPECTING_COMMAND;
break;
}
case COM_EXPECTING_COMMAND:
if(c == '\r' || c == '\n') {
com.command_buf[com.command_idx] = '\0';
com.command_idx = 0;
int rv = com_execute(com.command_buf);
if(rv == COM_ACK) {
com_send_resp('A');
} else if(rv == COM_ACK_SENSORS) {
com_send_resp('Q');
com_send_sensors();
} else if(rv == COM_ACK_DELTA) {
com_send_resp('M');
com_send_delta();
} else if(rv == COM_ACK_VELOCITY) {
com_send_resp('V');
com_send_velocity();
} else if (rv == COM_NACK_UNKNOWN) {
com_send_resp('U');
} else if (rv == COM_NACK_FORMAT) {
com_send_resp('F');
} else if (rv == COM_NACK_RANGE) {
com_send_resp('R');
} else if (rv < 0){
com_send_resp('N');
}
break;
} else {
com.command_buf[com.command_idx++] = c;
}
break;
break;
}
}
......@@ -852,7 +858,7 @@ int main(void)
GPIO_PUPD_NONE,
GPIO15);
u_printf("robo v5\r\n\r\n");
u_printf("robo v5.1\r\n\r\n");
/* { */
/* mc_t drv[] = { 0x0, -(0xffff/2) }; */
......@@ -879,7 +885,7 @@ int main(void)
/* ADC_CR(ADC) |= ADC_CR_ADSTART; */
/* } */
int sign = +1;
/* int sign = +1; */
/* timer_set_oc_value(TIM14, TIM_OC1, SC_PER/2); */
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment