2 * Copyright Droids Corporation
3 * Olivier Matz <zer0@droids-corp.org>
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.
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.
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
19 * Revision : $Id: main.c,v 1.6 2009-03-15 20:08:51 zer0 Exp $
24 * Cmdline interface for AX12. Use the PC to command a daisy-chain of
25 * AX12 actuators with a nice command line interface.
27 * The circuit should be as following:
30 * | uart0|------->--- PC (baudrate=57600)
34 * | uart1|---->---+-- AX12 (baudrate 1 000 000)
38 * Note that RX and TX pins of UART1 are connected together to provide
39 * a half-duplex UART emulation.
47 #include <aversive/pgmspace.h>
48 #include <aversive/wait.h>
56 #include <encoders_microb.h>
58 #include <scheduler.h>
62 #include <control_system_manager.h>
70 /* for cmdline interface */
72 char prompt[RDLINE_PROMPT_SIZE];
73 extern parse_pgm_ctx_t main_ctx[];
75 /* structure defining the AX12 servo */
81 /* for storing mesures*/
82 uint8_t sample_tab[MAX_SAMPLE];
83 uint16_t sample_i = 0;
85 /******** For cmdline. See in commands.c for the list of commands. */
86 static void write_char(char c)
92 valid_buffer(const char * buf, uint8_t size)
95 ret = parse(main_ctx, buf);
96 if (ret == PARSE_AMBIGUOUS)
97 printf_P(PSTR("Ambiguous command\r\n"));
98 else if (ret == PARSE_NOMATCH)
99 printf_P(PSTR("Command not found\r\n"));
100 else if (ret == PARSE_BAD_ARGS)
101 printf_P(PSTR("Bad arguments\r\n"));
105 complete_buffer(const char * buf, char * dstbuf, uint8_t dstsize,
108 return complete(main_ctx, buf, state, dstbuf, dstsize);
111 /********************************* AX12 commands */
116 * We use synchronous access (not interrupt driven) to the hardware
117 * UART, because we have to be sure that the transmission/reception is
118 * really finished when we return from the functions.
120 * We don't use the CM-5 circuit as described in the AX12
121 * documentation, we simply connect TX and RX and use TXEN + RXEN +
122 * DDR to manage the port directions.
125 static volatile uint8_t ax12_state = AX12_STATE_READ;
126 extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
127 static volatile uint8_t ax12_nsent = 0;
129 /* Called by ax12 module to send a character on serial line. Count the
130 * number of transmitted bytes. It will be used in ax12_recv_char() to
131 * drop the bytes that we transmitted. */
132 static int8_t ax12_send_char(uint8_t c)
144 /* called by uart module when the character has been written in
145 * UDR. It does not mean that the byte is physically transmitted. */
146 static void ax12_send_callback(char c)
148 if (ax12_state == AX12_STATE_READ) {
149 /* disable TX when last byte is pushed. */
150 if (CIRBUF_IS_EMPTY(&g_tx_fifo[1]))
151 UCSR1B &= ~(1<<TXEN);
155 /* Called by ax12 module when we want to receive a char. Note that we
156 * also receive the bytes we sent ! So we need to drop them. */
157 static int16_t ax12_recv_char(void)
159 microseconds t = time_get_us2();
162 c = uart_recv_nowait(1);
170 if ((time_get_us2() - t) > 50000)
176 /* called by ax12 module when we want to switch serial line. As we
177 * work in interruption mode, this function can be called to switch
178 * back in read mode even if the bytes are not really transmitted on
179 * the line. That's why in this case we do nothing, we will fall back
180 * in read mode in any case when xmit is finished -- see in
181 * ax12_send_callback() -- */
182 static void ax12_switch_uart(uint8_t state)
186 if (state == AX12_STATE_WRITE) {
189 while (uart_recv_nowait(1) != -1);
191 ax12_state = AX12_STATE_WRITE;
196 if (CIRBUF_IS_EMPTY(&g_tx_fifo[1]))
197 UCSR1B &= ~(1<<TXEN);
198 ax12_state = AX12_STATE_READ;
203 /***********************/
205 void do_led_blink(void * dummy)
207 #if 1 /* simple blink */
219 /* called every 5 ms */
220 static void do_cs(void * dummy)
222 if (arm.flags & CS_ON)
223 cs_manage(&arm.cs_mot);
225 if (scanner.flags & CS_ON){
226 cs_manage(&scanner.cs_mot);
232 static void main_timer_interrupt(void)
234 static uint8_t cpt = 0;
235 static uint8_t encoder_running = 0;
246 encoders_microb_manage(NULL);
249 if ((cpt & 0x3) == 0)
250 scheduler_interrupt();
253 /* sending "pop" on uart0 resets the robot */
254 static void emergency(char c) {
255 static uint8_t i = 0;
257 if( (i == 0 && c == 'p') ||
258 (i == 1 && c == 'o') ||
259 (i == 2 && c == 'p') )
261 else if ( !(i == 1 && c == 'p') )
270 //#define SCANNER_STEP_TOUR (3525L)
272 /* called every 1 ms */
273 #define STEP_PER_POS 64L
274 #define PIX_PER_SCAN 80L
276 int32_t pos_start_scan;
278 int32_t last_tour_pos;
288 #define l_mirror 235.
292 //#define offset_a (75.*M_PI/180.)
295 /* get motor angle in radian; return mirror angle in radian, cos a sin a */
296 void ang2_a_mirror(float b, float * c_a, float* s_a, float* a)
299 float A, DELTA, B, D;
305 A = (l3*l3+(x2)*(x2)+(y2)*(y2)-l2*l2)/(2*l3);
307 DELTA = -(A*A-(x2)*(x2)-(y2)*(y2));
310 D = (x2)*(x2)+(y2)*(y2);
312 *c_a = (x2*A+y2*B)/D;
313 *s_a = -(x2*B-y2*A)/D;
315 *a = atan2(*s_a, *c_a);
318 /* get telemeter dist , cos a, sin a, a and return H, L of scanned point */
319 void ang2_H_L(float l_telemetre, float c_a, float s_a, float a, float *H, float *L)
322 d = h_mirror*c_a/s_a;
323 *H = (l_telemetre - l_mirror - d)*sin(2*a);
324 *L = l_mirror + d + *H/tan(2*a);
327 // d_telemetre = a * cm + b
328 #define TELEMETRE_A (16.76)
329 #define TELEMETRE_B (-476.)
331 static void do_adc(void * dummy)
337 int32_t pos, pos_tmp, last_pos;
341 float b, c_a, s_a, H, L, m_a;
347 mot_pos = encoders_microb_get_value((void *)SCANNER_ENC);
348 mot_pos-=pos_start_scan;
351 printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN);
352 //scanner.flags &= (~CS_ON);
354 mot_pos = SCANNER_STEP_TOUR*(encoders_microb_get_value((void *)SCANNER_ENC)/SCANNER_STEP_TOUR+1L);
355 cs_set_consign(&scanner.cs_mot, mot_pos);
356 //pwm_ng_set(SCANNER_MOT_PWM, 0);
360 a = adc_get_value( ADC_REF_AVCC | MUX_ADC0 );
361 //printf_P(PSTR("polling : ADC0 = %i\n"),a);
362 dist = (a-TELEMETRE_B)/TELEMETRE_A;
364 //printf_P(PSTR("enc val = %ld\r\n"), encoders_microb_get_value((void *)SCANNER_ENC));
367 //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a;
368 //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0;
371 tour_n = (mot_pos)/(SCANNER_STEP_TOUR);
372 tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
374 b = (2.*M_PI)*(float)tour_pos/(float)(SCANNER_STEP_TOUR);
375 ang2_a_mirror(b, &c_a, &s_a, &m_a);
376 ang2_H_L(dist, c_a, s_a, m_a, &H, &L);
378 H = H;//m_a*180/M_PI;
379 L = L;//(L-100)*PIX_PER_SCAN;
382 //printf_P(PSTR("%f %f\r\n"), dist, m_a*180/M_PI);
383 //printf_P(PSTR("%f %f\r\n"), m_a*180/M_PI, b*180/M_PI);
385 //printf_P(PSTR("%d %f\r\n"), a, b*180/M_PI);
386 //printf_P(PSTR("%f %f\r\n"), H, L);
387 printf_P(PSTR("%f %f %f\r\n"), H, m_a*180/M_PI, offset_a);
389 //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
390 tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
393 tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
398 //pos = (tour_n*SCANNER_STEP_TOUR + tour_pos)/STEP_PER_POS;
399 pos= tour_n*PIX_PER_SCAN+tour_pos;
400 last_pos= last_tour_n*PIX_PER_SCAN+last_tour_pos;
406 if (pos <MAX_SAMPLE)// && tour_n%2)
407 //sample_tab[pos] = a>0xff?0xFF:a;
408 //sample_tab[(int)L] = H ;
412 if (last_tour_n == tour_n){
418 for (pos_tmp=pos;pos_tmp< last_pos;pos_tmp++){
419 if (pos_tmp <MAX_SAMPLE)// && tour_n%2)
420 //sample_tab[pos_tmp] = a>0xff?0xFF:a;
429 last_tour_n = tour_n;
430 last_tour_pos = tour_pos;
433 //printf("pos : %ld\r\n", pos);
434 //sample_tab[sample_i] = a>0x1ff?0x1FF:a;
436 //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
439 if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
440 sample_tab[pos] = 0xffff;
448 const char * history;
459 memset(&arm, 0, sizeof(struct arm));
462 pid_init(&arm.pid_mot);
463 pid_set_gains(&arm.pid_mot, 80, 5, 250);
464 pid_set_maximums(&arm.pid_mot, 0, 10000, 4095);
465 pid_set_out_shift(&arm.pid_mot, 6);
466 pid_set_derivate_filter(&arm.pid_mot, 6);
470 quadramp_init(&arm.qr_mot);
471 quadramp_set_1st_order_vars(&arm.qr_mot, 200, 200); /* set speed */
472 quadramp_set_2nd_order_vars(&arm.qr_mot, 20, 20); /* set accel */
475 memset(&scanner, 0, sizeof(struct arm));
477 cs_init(&arm.cs_mot);
478 cs_set_consign_filter(&arm.cs_mot, quadramp_do_filter, &arm.qr_mot);
479 cs_set_correct_filter(&arm.cs_mot, pid_do_filter, &arm.pid_mot);
480 cs_set_process_in(&arm.cs_mot, pwm_ng_set, ARM_MOT_PWM);
481 cs_set_process_out(&arm.cs_mot, encoders_microb_get_value, ARM_ENC);
482 cs_set_consign(&arm.cs_mot, 0);
486 pid_init(&scanner.pid_mot);
487 pid_set_gains(&scanner.pid_mot, 80, 5, 250);
488 pid_set_maximums(&scanner.pid_mot, 0, 10000, 2047);
489 pid_set_out_shift(&scanner.pid_mot, 6);
490 pid_set_derivate_filter(&scanner.pid_mot, 6);
493 quadramp_init(&scanner.qr_mot);
494 quadramp_set_1st_order_vars(&scanner.qr_mot, 40, 40); /* set speed */
495 quadramp_set_2nd_order_vars(&scanner.qr_mot, 20, 20); /* set accel */
498 cs_init(&scanner.cs_mot);
499 cs_set_consign_filter(&scanner.cs_mot, quadramp_do_filter, &scanner.qr_mot);
500 cs_set_correct_filter(&scanner.cs_mot, pid_do_filter, &scanner.pid_mot);
501 cs_set_process_in(&scanner.cs_mot, pwm_ng_set, SCANNER_MOT_PWM);
502 cs_set_process_out(&scanner.cs_mot, encoders_microb_get_value, SCANNER_ENC);
503 cs_set_consign(&scanner.cs_mot, 0);
505 //scanner.flags |= CS_ON;
511 spi_init(SPI_MODE_MASTER, SPI_FORMAT_2, SPI_CLK_RATE_16);
512 spi_set_data_order(SPI_MSB_FIRST);
513 spi_register_ss_line(&SS_PORT, SS_BIT);
517 /* Initialize full duplex uart direction port */
518 sbi(PORTD,3); /* pullup */
520 /* disable rx intr, needed for AX12 !! */
521 //UCSRnB &= ~( (1 << RXCIE) | (1 << UDRIE) | (1 << TXCIE) );
523 ax12_switch_uart(AX12_STATE_READ);
524 fdevopen(uart0_dev_send, uart0_dev_recv);
525 uart_register_rx_event(0, emergency);
529 /* i2c_protocol_init(); */
530 i2c_init(I2C_MODE_MASTER, 0/* I2C_MAIN_ADDR */);
531 /* i2c_register_recv_event(i2c_recvevent); */
532 /* i2c_register_send_event(i2c_sendevent); */
533 /* scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL, */
534 /* 8000L / SCHEDULER_UNIT, I2C_POLL_PRIO); */
538 AX12_set_hardware_send(&ax12, ax12_send_char);
539 AX12_set_hardware_recv(&ax12, ax12_recv_char);
540 AX12_set_hardware_switch(&ax12, ax12_switch_uart);
541 uart_register_tx_event(1, ax12_send_callback);
544 encoders_microb_init();
548 timer0_register_OV_intr(main_timer_interrupt);
552 scheduler_add_periodical_event_priority(do_led_blink, NULL,
553 100000L / SCHEDULER_UNIT,
556 PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
557 TIMER1_PRESCALER_DIV_1);
559 PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
560 TIMER1_PRESCALER_DIV_1);
562 /* pwm_ng_timer_8bits_init(2, TIMER_8_MODE_PWM, */
563 /* TIMER1_PRESCALER_DIV_1); */
564 PWM_NG_INIT16(&arm.pwm1A, 1, A, 10, PWM_NG_MODE_SIGNED |
565 PWM_NG_MODE_SIGN_INVERTED, &PORTB, 0);
566 PWM_NG_INIT16(&arm.pwm1B, 1, B, 10, PWM_NG_MODE_SIGNED,
568 PWM_NG_INIT16(&arm.pwm3C, 3, C, 10, PWM_NG_MODE_SIGNED,
570 /* PWM_NG_INIT8(&arm.pwm2, 2, 10, PWM_NG_MODE_SIGNED, */
574 scheduler_add_periodical_event_priority(do_cs, NULL,
575 CS_PERIOD / SCHEDULER_UNIT,
581 scheduler_add_periodical_event_priority(do_adc, NULL,
582 2000L / SCHEDULER_UNIT,
587 /* arm xy init matrix */
591 time_init(TIME_PRIO);
597 printf_P(PSTR("Coucou\r\n"));
599 /* set status return level to 2 and torque to 0 */
600 AX12_write_int(&ax12,0xFE, AA_TORQUE_ENABLE, 0x00);
601 AX12_write_byte(&ax12, 0xFE, AA_STATUS_RETURN_LEVEL, 2);
603 rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
604 snprintf(prompt, sizeof(prompt), "ax12 > ");
605 rdline_newline(&rdl, prompt);
609 c = uart_recv_nowait(0);
612 ret = rdline_char_in(&rdl, c);
613 if (ret != 2 && ret != 0) {
614 history = rdline_get_buffer(&rdl);
615 if (strlen(history) > 1)
616 rdline_add_history(&rdl, history);
617 rdline_newline(&rdl, prompt);