projects/microb2009/tests/arm_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.6 2009-03-15 20:08:51 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 <i2c.h>
  52 #include <ax12.h>
  53 #include <parse.h>
  54 #include <rdline.h>
  55 #include <pwm_ng.h>
  56 #include <encoders_microb.h>
  57 #include <timer.h>
  58 #include <scheduler.h>
  59 #include <pid.h>
  60 #include <time.h>
  61 #include <quadramp.h>
  62 #include <control_system_manager.h>
  63 #include <adc.h>
  64 #include <spi.h>
  65
  66 #include "main.h"
  67
  68 #include "arm_xy.h"
  69
  70 /* for cmdline interface */
  71 struct rdline rdl;
  72 char prompt[RDLINE_PROMPT_SIZE];
  73 extern parse_pgm_ctx_t main_ctx[];
  74
  75 /* structure defining the AX12 servo */
  76 AX12 ax12;
  77 struct arm arm;
  78
  79 struct arm scanner;
  80
  81 /* for storing mesures*/
  82 uint8_t sample_tab[MAX_SAMPLE];
  83 uint16_t sample_i = 0;
  84
  85 /******** For cmdline. See in commands.c for the list of commands. */
  86 static void write_char(char c)
  87 {
  88         uart_send(0, c);
  89 }
  90
  91 static void
  92 valid_buffer(const char * buf, uint8_t size)
  93 {
  94         int8_t ret;
  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"));
 102 }
 103
 104 static int8_t
 105 complete_buffer(const char * buf, char * dstbuf, uint8_t dstsize,
 106                 int16_t * state)
 107 {
 108         return complete(main_ctx, buf, state, dstbuf, dstsize);
 109 }
 110
 111 /********************************* AX12 commands */
 112
 113 /*
 114  * --- use uart1
 115  *
 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.
 119  *
 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.
 123  */
 124
 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;
 128
 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)
 133 {
 134         uart_send(1, c);
 135         ax12_nsent++;
 136         return 0;
 137 }
 138
 139 /* for atmega256 */
 140 #ifndef TXEN
 141 #define TXEN TXEN0
 142 #endif
 143
 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)
 147 {
 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);
 152         }
 153 }
 154
 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)
 158 {
 159         microseconds t = time_get_us2();
 160         int c;
 161         while (1) {
 162                 c = uart_recv_nowait(1);
 163                 if (c != -1) {
 164                         if (ax12_nsent == 0)
 165                                 return c;
 166                         ax12_nsent --;
 167                 }
 168
 169                 /* 50 ms timeout */
 170                 if ((time_get_us2() - t) > 50000)
 171                         return -1;
 172         }
 173         return c;
 174 }
 175
 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)
 183 {
 184         uint8_t flags;
 185
 186         if (state == AX12_STATE_WRITE) {
 187                 IRQ_LOCK(flags);
 188                 ax12_nsent=0;
 189                 while (uart_recv_nowait(1) != -1);
 190                 UCSR1B |= (1<<TXEN);
 191                 ax12_state = AX12_STATE_WRITE;
 192                 IRQ_UNLOCK(flags);
 193         }
 194         else {
 195                 IRQ_LOCK(flags);
 196                 if (CIRBUF_IS_EMPTY(&g_tx_fifo[1]))
 197                         UCSR1B &= ~(1<<TXEN);
 198                 ax12_state = AX12_STATE_READ;
 199                 IRQ_UNLOCK(flags);
 200         }
 201 }
 202
 203 /***********************/
 204
 205 void do_led_blink(void * dummy)
 206 {
 207 #if 1 /* simple blink */
 208         static uint8_t a=0;
 209
 210         if(a)
 211                 LED1_ON();
 212         else
 213                 LED1_OFF();
 214
 215         a = !a;
 216 #endif
 217 }
 218
 219 /* called every 5 ms */
 220 static void do_cs(void * dummy)
 221 {
 222         if (arm.flags & CS_ON)
 223                 cs_manage(&arm.cs_mot);
 224
 225         if (scanner.flags & CS_ON){
 226                 cs_manage(&scanner.cs_mot);
 227
 228         }
 229
 230 }
 231
 232 static void main_timer_interrupt(void)
 233 {
 234         static uint8_t cpt = 0;
 235         static uint8_t encoder_running = 0;
 236
 237         cpt++;
 238
 239         /* log ? */
 240         if (encoder_running)
 241                 return;
 242
 243         encoder_running = 1;
 244         sei();
 245
 246         encoders_microb_manage(NULL);
 247         encoder_running = 0;
 248
 249         if ((cpt & 0x3) == 0)
 250                 scheduler_interrupt();
 251 }
 252
 253 /* sending "pop" on uart0 resets the robot */
 254 static void emergency(char c) {
 255         static uint8_t i = 0;
 256
 257         if( (i == 0 && c == 'p') ||
 258             (i == 1 && c == 'o') ||
 259             (i == 2 && c == 'p') )
 260                 i++;
 261         else if ( !(i == 1 && c == 'p') )
 262                 i = 0;
 263         if(i == 3){
 264                 reset();
 265                 //PORTG|=0x3;
 266
 267         }
 268 }
 269
 270 //#define SCANNER_STEP_TOUR (3525L)
 271
 272 /* called every 1 ms */
 273 #define STEP_PER_POS 64L
 274 #define PIX_PER_SCAN 80L
 275
 276 int32_t pos_start_scan;
 277 int32_t last_tour_n;
 278 int32_t last_tour_pos;
 279
 280
 281
 282
 283 #define X  45.
 284 #define Y  -11.
 285 #define l1  9.
 286 #define l2  21.13
 287 #define l3  47.14
 288 #define l_mirror  235.
 289 #define h_mirror  15.
 290
 291
 292 //#define offset_a (75.*M_PI/180.)
 293 float offset_a;
 294
 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)
 297 {
 298         float x2, y2;
 299         float A, DELTA, B, D;
 300
 301         b+=offset_a;
 302         x2 = X + l1*cos(b);
 303         y2 = Y + l1*sin(b);
 304
 305         A = (l3*l3+(x2)*(x2)+(y2)*(y2)-l2*l2)/(2*l3);
 306
 307         DELTA = -(A*A-(x2)*(x2)-(y2)*(y2));
 308         B = sqrt(DELTA);
 309
 310         D = (x2)*(x2)+(y2)*(y2);
 311
 312         *c_a = (x2*A+y2*B)/D;
 313         *s_a = -(x2*B-y2*A)/D;
 314
 315         *a = atan2(*s_a, *c_a);
 316 }
 317
 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)
 320 {
 321         float d;
 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);
 325 }
 326
 327 // d_telemetre = a * cm + b
 328 #define TELEMETRE_A (16.76)
 329 #define TELEMETRE_B (-476.)
 330
 331 static void do_adc(void * dummy)
 332 {
 333
 334         int16_t a;
 335         int32_t tour_n;
 336         int32_t tour_pos;
 337         int32_t pos, pos_tmp, last_pos;
 338         int32_t mot_pos;
 339         float dist;
 340
 341         float b, c_a, s_a, H, L, m_a;
 342
 343
 344         if (sample_i==0)
 345                 return;
 346
 347         mot_pos = encoders_microb_get_value((void *)SCANNER_ENC);
 348         mot_pos-=pos_start_scan;
 349
 350         if (sample_i==1){
 351                 printf_P(PSTR("dump end enc %ld %d \r\n"), mot_pos, PIX_PER_SCAN);
 352                 //scanner.flags &= (~CS_ON);
 353
 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);
 357
 358
 359         }
 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;
 363
 364         //printf_P(PSTR("enc val = %ld\r\n"),   encoders_microb_get_value((void *)SCANNER_ENC));
 365
 366
 367         //sample_tab[MAX_SAMPLE-sample_i] = a>0x1ff?0x1FF:a;
 368         //sample_tab[MAX_SAMPLE-sample_i] |= PINF&2?0x200:0;
 369
 370
 371         tour_n = (mot_pos)/(SCANNER_STEP_TOUR);
 372         tour_pos = (mot_pos)%(SCANNER_STEP_TOUR);
 373
 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);
 377
 378         H = H;//m_a*180/M_PI;
 379         L = L;//(L-100)*PIX_PER_SCAN;
 380
 381
 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);
 384
 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);
 388         if (tour_n%2){
 389                 //tour_pos = ((SCANNER_STEP_TOUR/2)-tour_pos);
 390                 tour_pos = (tour_pos*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
 391         }
 392         else{
 393                 tour_pos = ((SCANNER_STEP_TOUR-tour_pos)*PIX_PER_SCAN)/(SCANNER_STEP_TOUR);
 394         }
 395
 396
 397
 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;
 401
 402         //a-=0x100;
 403         a-=200;
 404         //a/=10;
 405
 406         if (pos <MAX_SAMPLE)// && tour_n%2)
 407                 //sample_tab[pos] =  a>0xff?0xFF:a;
 408                 //sample_tab[(int)L] = H ;
 409                 sample_tab[pos] = H;
 410                 nop();
 411
 412         if (last_tour_n == tour_n){
 413                 if (pos > last_pos){
 414                         pos_tmp = pos;
 415                         pos = last_pos;
 416                         last_pos = pos_tmp;
 417                 }
 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;
 421                                 nop();
 422
 423                 }
 424
 425         }
 426
 427
 428
 429         last_tour_n = tour_n;
 430         last_tour_pos = tour_pos;
 431
 432
 433         //printf("pos : %ld\r\n", pos);
 434         //sample_tab[sample_i] =  a>0x1ff?0x1FF:a;
 435
 436         //sample_ok_tab[MAX_SAMPLE-sample_i] = PORTF&2;
 437
 438         /*
 439         if (((pos <MAX_SAMPLE)) && (tour_pos<=(SCANNER_STEP_TOUR/2)))
 440                 sample_tab[pos] = 0xffff;
 441         */
 442         sample_i--;
 443 }
 444
 445 int main(void)
 446 {
 447         int c;
 448         const char * history;
 449         int8_t ret;
 450
 451         /* brake */
 452         DDRG=0x3;
 453         PORTG=0x0;
 454
 455         /* LEDS */
 456         DDRE=0x0C;
 457
 458         LED1_OFF();
 459         memset(&arm, 0, sizeof(struct arm));
 460
 461         /* PID */
 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);
 467
 468
 469         /* QUADRAMP */
 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 */
 473
 474         /* CS */
 475         memset(&scanner, 0, sizeof(struct arm));
 476
 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);
 483
 484
 485
 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);
 491
 492
 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 */
 496
 497
 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);
 504
 505         //scanner.flags |= CS_ON;
 506
 507
 508
 509 #if 0
 510         /* SPI */
 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);
 514 #endif
 515
 516         /* UART */
 517         /* Initialize full duplex uart direction port */
 518         sbi(PORTD,3); /* pullup */
 519         uart_init();
 520         /* disable rx intr, needed for AX12 !! */
 521         //UCSRnB &= ~( (1 << RXCIE) | (1 << UDRIE) | (1 << TXCIE) );
 522
 523         ax12_switch_uart(AX12_STATE_READ);
 524         fdevopen(uart0_dev_send, uart0_dev_recv);
 525         uart_register_rx_event(0, emergency);
 526
 527         /* I2C */
 528         wait_ms(50);
 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); */
 535
 536         /* AX12 */
 537         AX12_init(&ax12);
 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);
 542
 543         /* ENCODERS */
 544         encoders_microb_init();
 545
 546         /* TIMER */
 547         timer_init();
 548         timer0_register_OV_intr(main_timer_interrupt);
 549
 550         /* SCHEDULER */
 551         scheduler_init();
 552         scheduler_add_periodical_event_priority(do_led_blink, NULL,
 553                                                 100000L / SCHEDULER_UNIT,
 554                                                 LED_PRIO);
 555         /* PWM */
 556         PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
 557                                  TIMER1_PRESCALER_DIV_1);
 558
 559         PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
 560                                  TIMER1_PRESCALER_DIV_1);
 561
 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,
 567                       &PORTB, 1);
 568         PWM_NG_INIT16(&arm.pwm3C, 3, C, 10, PWM_NG_MODE_SIGNED,
 569                       &PORTE, 4);
 570 /*      PWM_NG_INIT8(&arm.pwm2, 2, 10, PWM_NG_MODE_SIGNED, */
 571 /*                    &PORTB, 2); */
 572
 573         /* CS EVENT */
 574         scheduler_add_periodical_event_priority(do_cs, NULL,
 575                                                 CS_PERIOD / SCHEDULER_UNIT,
 576                                                 CS_PRIO);
 577
 578         /* ADC EVENT */
 579
 580         adc_init();
 581         scheduler_add_periodical_event_priority(do_adc, NULL,
 582                                                 2000L / SCHEDULER_UNIT,
 583                                                 CS_PRIO-1);
 584
 585         wait_ms(200);
 586
 587         /* arm xy init matrix */
 588         init_arm_matrix();
 589
 590         /* TIME */
 591         time_init(TIME_PRIO);
 592
 593         wait_ms(200);
 594
 595         sei();
 596
 597         printf_P(PSTR("Coucou\r\n"));
 598
 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);
 602
 603         rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
 604         snprintf(prompt, sizeof(prompt), "ax12 > ");
 605         rdline_newline(&rdl, prompt);
 606
 607
 608         while (1) {
 609                 c = uart_recv_nowait(0);
 610                 if (c == -1)
 611                         continue;
 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);
 618                 }
 619         }
 620
 621         return 0;
 622 }