modules/devices/servo/ax12/test/main.c

   1 /*
   2  *  Copyright Droids Corporation
   3  *  Olivier Matz <zer0@droids-corp.org>
   4  *
   5  *  This program is free software; you can redistribute it and/or modify
   6  *  it under the terms of the GNU General Public License as published by
   7  *  the Free Software Foundation; either version 2 of the License, or
   8  *  (at your option) any later version.
   9  *
  10  *  This program is distributed in the hope that it will be useful,
  11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  *  GNU General Public License for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License
  16  *  along with this program; if not, write to the Free Software
  17  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18  *
  19  *  Revision : $Id: main.c,v 1.1.4.4 2008-12-27 16:29:08 zer0 Exp $
  20  *
  21  */
  22
  23 /*
  24  * Cmdline interface for AX12. Use the PC to command a daisy-chain of
  25  * AX12 actuators with a nice command line interface.
  26  *
  27  * The circuit should be as following:
  28  *
  29  *    |----------|
  30  *    |     uart0|------->--- PC (baudrate=57600)
  31  *    |          |-------<---
  32  *    | atmega128|
  33  *    |          |
  34  *    |     uart1|---->---+-- AX12 (baudrate 1 000 000)
  35  *    |          |----<---|
  36  *    |----------|
  37  *
  38  * Note that RX and TX pins of UART1 are connected together to provide
  39  * a half-duplex UART emulation.
  40  *
  41  */
  42
  43 #include <stdio.h>
  44 #include <string.h>
  45
  46 #include <aversive.h>
  47 #include <aversive/pgmspace.h>
  48 #include <aversive/wait.h>
  49
  50 #include <uart.h>
  51 #include <ax12.h>
  52 #include <parse.h>
  53 #include <rdline.h>
  54 #include <scheduler.h>
  55 #include <time.h>
  56 #include <timer.h>
  57
  58 /* for cmdline interface */
  59 struct rdline rdl;
  60 char prompt[RDLINE_PROMPT_SIZE];
  61 extern parse_pgm_ctx_t main_ctx[];
  62
  63 /* structure defining the AX12 servo */
  64 AX12 ax12;
  65
  66 /******** For cmdline. See in commands.c for the list of commands. */
  67 static void write_char(char c)
  68 {
  69         uart_send(0, c);
  70 }
  71
  72 static void
  73 valid_buffer(const char * buf, uint8_t size)
  74 {
  75         int8_t ret;
  76         ret = parse(main_ctx, buf);
  77         if (ret == PARSE_AMBIGUOUS)
  78                 printf_P(PSTR("Ambiguous command\r\n"));
  79         else if (ret == PARSE_NOMATCH)
  80                 printf_P(PSTR("Command not found\r\n"));
  81         else if (ret == PARSE_BAD_ARGS)
  82                 printf_P(PSTR("Bad arguments\r\n"));
  83 }
  84
  85 static int8_t
  86 complete_buffer(const char * buf, char * dstbuf, uint8_t dstsize,
  87                 int16_t * state)
  88 {
  89         return complete(main_ctx, buf, state, dstbuf, dstsize);
  90 }
  91
  92 /********************************* AX12 commands */
  93
  94 /*
  95  * --- use uart1
  96  *
  97  * We use synchronous access (not interrupt driven) to the hardware
  98  * UART, because we have to be sure that the transmission/reception is
  99  * really finished when we return from the functions.
 100  *
 101  * We don't use the CM-5 circuit as described in the AX12
 102  * documentation, we simply connect TX and RX and use TXEN + RXEN +
 103  * DDR to manage the port directions.
 104  */
 105
 106 static volatile uint8_t ax12_state = AX12_STATE_READ;
 107 extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
 108 static volatile uint8_t ax12_nsent = 0;
 109
 110 /* Called by ax12 module to send a character on serial line. Count the
 111  * number of transmitted bytes. It will be used in ax12_recv_char() to
 112  * drop the bytes that we transmitted. */
 113 static int8_t ax12_send_char(uint8_t c)
 114 {
 115         uart_send(1, c);
 116         ax12_nsent++;
 117         return 0;
 118 }
 119
 120 /* called by uart module when the character has been written in
 121  * UDR. It does not mean that the byte is physically transmitted. */
 122 static void ax12_send_callback(char c)
 123 {
 124         if (ax12_state == AX12_STATE_READ) {
 125                 /* disable TX when last byte is pushed. */
 126                 if (CIRBUF_IS_EMPTY(&g_tx_fifo[1]))
 127                         UCSR1B &= ~(1<<TXEN);
 128         }
 129 }
 130
 131 /* Called by ax12 module when we want to receive a char. Note that we
 132  * also receive the bytes we sent ! So we need to drop them. */
 133 static int16_t ax12_recv_char(void)
 134 {
 135         microseconds t = time_get_us2();
 136         int c;
 137         while (1) {
 138                 c = uart_recv_nowait(1);
 139                 if (c != -1) {
 140                         if (ax12_nsent == 0)
 141                                 return c;
 142                         ax12_nsent --;
 143                 }
 144
 145                 /* 50 ms timeout */
 146                 if ((time_get_us2() - t) > 50000)
 147                         return -1;
 148         }
 149         return c;
 150 }
 151
 152 /* called by ax12 module when we want to switch serial line. As we
 153  * work in interruption mode, this function can be called to switch
 154  * back in read mode even if the bytes are not really transmitted on
 155  * the line. That's why in this case we do nothing, we will fall back
 156  * in read mode in any case when xmit is finished -- see in
 157  * ax12_send_callback() -- */
 158 static void ax12_switch_uart(uint8_t state)
 159 {
 160         uint8_t flags;
 161
 162         if (state == AX12_STATE_WRITE) {
 163                 IRQ_LOCK(flags);
 164                 ax12_nsent=0;
 165                 UCSR1B |= (1<<TXEN);
 166                 ax12_state = AX12_STATE_WRITE;
 167                 IRQ_UNLOCK(flags);
 168         }
 169         else {
 170                 IRQ_LOCK(flags);
 171                 if (CIRBUF_IS_EMPTY(&g_tx_fifo[1]))
 172                         UCSR1B &= ~(1<<TXEN);
 173                 ax12_state = AX12_STATE_READ;
 174                 IRQ_UNLOCK(flags);
 175         }
 176 }
 177
 178 static void main_timer_interrupt(void)
 179 {
 180         static uint8_t cpt = 0;
 181
 182         cpt++;
 183
 184         if ((cpt & 0x3) == 0)
 185                 scheduler_interrupt();
 186 }
 187
 188 /***********************/
 189
 190 int main(void)
 191 {
 192         int c;
 193         const char * history;
 194         int8_t ret;
 195
 196         wait_ms(1000);
 197
 198         uart_init();
 199
 200         ax12_switch_uart(AX12_STATE_READ);
 201         fdevopen(uart0_dev_send, uart0_dev_recv);
 202
 203         /* AX12 */
 204         AX12_init(&ax12);
 205         AX12_set_hardware_send(&ax12, ax12_send_char);
 206         AX12_set_hardware_recv(&ax12, ax12_recv_char);
 207         AX12_set_hardware_switch(&ax12, ax12_switch_uart);
 208         uart_register_tx_event(1, ax12_send_callback);
 209
 210         /* TIMER */
 211         timer_init();
 212         timer0_register_OV_intr(main_timer_interrupt);
 213
 214         /* SCHEDULER */
 215         scheduler_init();
 216
 217         sei();
 218
 219         /* set status return level to 2 and torque to 0 */
 220         AX12_write_int(&ax12,0xFE, AA_TORQUE_ENABLE, 0x00);
 221         AX12_write_byte(&ax12, 0xFE, AA_STATUS_RETURN_LEVEL, 2);
 222
 223         rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
 224         snprintf(prompt, sizeof(prompt), "ax12 > ");
 225         rdline_newline(&rdl, prompt);
 226
 227         /* waiting for commands now... */
 228
 229         while (1) {
 230                 c = uart_recv_nowait(0);
 231                 if (c == -1)
 232                         continue;
 233                 ret = rdline_char_in(&rdl, c);
 234                 if (ret != 2 && ret != 0) {
 235                         history = rdline_get_buffer(&rdl);
 236                         if (strlen(history) > 1)
 237                                 rdline_add_history(&rdl, history);
 238                         rdline_newline(&rdl, prompt);
 239                 }
 240         }
 241
 242         return 0;
 243 }