projects/microb2009/tests/atm2560/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.2 2009-02-20 21:10:01 zer0 Exp $
  20  *
  21  */
  22
  23 #include <stdio.h>
  24 #include <string.h>
  25
  26 #include <aversive.h>
  27 #include <aversive/pgmspace.h>
  28 #include <aversive/wait.h>
  29
  30 #include <parse.h>
  31 #include <rdline.h>
  32 #include <ax12.h>
  33 #include <uart.h>
  34 #include <pwm_ng.h>
  35 #include <timer.h>
  36 #include <scheduler.h>
  37 #include <pid.h>
  38 #include <time.h>
  39 #include <quadramp.h>
  40 #include <control_system_manager.h>
  41
  42 #include "main.h"
  43
  44 /* ---- with 16 Mhz quartz
  45  * CKSEL 3-0 : 0110
  46  * SUT 1-0 : 00
  47  * CKDIV8 : 1
  48  * ---- bootloader
  49  * BOOTZ 1-0 : 01 (4K bootloader)
  50  * BOOTRST : 0 (reset on bootloader)
  51  */
  52
  53 /* for cmdline interface */
  54 struct rdline rdl;
  55 char prompt[RDLINE_PROMPT_SIZE];
  56 extern parse_pgm_ctx_t main_ctx[];
  57
  58 AX12 ax12;
  59 struct arm arm;
  60
  61
  62 /******** For cmdline. See in commands.c for the list of commands. */
  63 static void write_char(char c)
  64 {
  65         uart_send(3, c);
  66 }
  67
  68 static void
  69 valid_buffer(const char * buf, uint8_t size)
  70 {
  71         int8_t ret;
  72         ret = parse(main_ctx, buf);
  73         if (ret == PARSE_AMBIGUOUS)
  74                 printf_P(PSTR("Ambiguous command\r\n"));
  75         else if (ret == PARSE_NOMATCH)
  76                 printf_P(PSTR("Command not found\r\n"));
  77         else if (ret == PARSE_BAD_ARGS)
  78                 printf_P(PSTR("Bad arguments\r\n"));
  79 }
  80
  81 static int8_t
  82 complete_buffer(const char * buf, char * dstbuf, uint8_t dstsize,
  83                 int16_t * state)
  84 {
  85         return complete(main_ctx, buf, state, dstbuf, dstsize);
  86 }
  87
  88
  89 /***********************/
  90
  91 void do_led_blink(void * dummy)
  92 {
  93 #if 1 /* simple blink */
  94         static uint8_t a=0;
  95
  96         if(a)
  97                 LED1_ON();
  98         else
  99                 LED1_OFF();
 100
 101         a = !a;
 102 #endif
 103 }
 104
 105 /* called every 5 ms */
 106 static void do_cs(void * dummy)
 107 {
 108         if (arm.flags & CS_ON)
 109                 cs_manage(&arm.cs_mot);
 110 }
 111
 112 static void main_timer_interrupt(void)
 113 {
 114         static uint8_t cpt = 0;
 115
 116         cpt++;
 117         sei();
 118
 119         if ((cpt & 0x3) == 0)
 120                 scheduler_interrupt();
 121 }
 122
 123 /* sending "pop" on uart0 resets the robot */
 124 static void emergency(char c)
 125 {
 126         static uint8_t i = 0;
 127
 128         if ((i == 0 && c == 'p') ||
 129             (i == 1 && c == 'o') ||
 130             (i == 2 && c == 'p'))
 131                 i++;
 132         else if ( !(i == 1 && c == 'p') )
 133                 i = 0;
 134         if (i == 3)
 135                 reset();
 136 }
 137
 138 int main(void)
 139 {
 140         int c;
 141         const char *history, *buffer;
 142         int8_t ret, same = 0;
 143
 144         /* brake */
 145         DDRJ = 0xF0;
 146         PORTJ = 0;
 147
 148         /* CPLD on XXX */
 149
 150         /* LEDS */
 151         DDRJ |= 0x0c;
 152         DDRL = 0xc0;
 153         LED1_OFF();
 154         memset(&arm, 0, sizeof(struct arm));
 155
 156         /* PID */
 157         pid_init(&arm.pid_mot);
 158         pid_set_gains(&arm.pid_mot, 20, 0, 0);
 159         pid_set_maximums(&arm.pid_mot, 0, 30000, 4095);
 160         pid_set_out_shift(&arm.pid_mot, 6);
 161         pid_set_derivate_filter(&arm.pid_mot, 6);
 162
 163         /* QUADRAMP */
 164         quadramp_init(&arm.qr_mot);
 165         quadramp_set_1st_order_vars(&arm.qr_mot, 800, 800); /* set speed */
 166         quadramp_set_2nd_order_vars(&arm.qr_mot, 20, 20); /* set accel */
 167
 168         /* CS */
 169 /*      cs_init(&arm.cs_mot); */
 170 /*      cs_set_consign_filter(&arm.cs_mot, quadramp_do_filter, &arm.qr_mot); */
 171 /*      cs_set_correct_filter(&arm.cs_mot, pid_do_filter, &arm.pid_mot); */
 172 /*      cs_set_process_in(&arm.cs_mot, pwm_ng_set, ARM_MOT_PWM); */
 173 /*      cs_set_process_out(&arm.cs_mot, encoders_microb_get_value, ARM_ENC); */
 174 /*      cs_set_consign(&arm.cs_mot, 0); */
 175
 176 #if 0
 177         /* SPI */
 178         spi_init(SPI_MODE_MASTER, SPI_FORMAT_2, SPI_CLK_RATE_16);
 179         spi_set_data_order(SPI_MSB_FIRST);
 180         spi_register_ss_line(&SS_PORT, SS_BIT);
 181 #endif
 182
 183
 184         uart_init();
 185         fdevopen(uart3_dev_send, uart3_dev_recv);
 186         uart_register_rx_event(3, emergency);
 187
 188         /* TIMER */
 189         timer_init();
 190         timer0_register_OV_intr(main_timer_interrupt);
 191
 192         /* SCHEDULER */
 193         scheduler_init();
 194         scheduler_add_periodical_event_priority(do_led_blink, NULL,
 195                                                 100000L / SCHEDULER_UNIT,
 196                                                 LED_PRIO);
 197
 198         /* PWM */
 199         PWM_NG_TIMER_8BITS_INIT_B(2, TIMER_8_MODE_PWM,
 200                                   TIMER2_PRESCALER_DIV_1);
 201         PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
 202                                  TIMER1_PRESCALER_DIV_1);
 203
 204         PWM_NG_INIT8_B(&arm.pwm1_2A, 2, A, 8, PWM_NG_MODE_SIGNED,
 205                      &PORTD, 4);
 206         PWM_NG_INIT16(&arm.pwm2_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
 207                       &PORTD, 5);
 208         PWM_NG_INIT16(&arm.pwm3_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
 209                       &PORTD, 6);
 210         PWM_NG_INIT16(&arm.pwm4_1C, 1, C, 10, PWM_NG_MODE_SIGNED,
 211                       &PORTD, 7);
 212
 213
 214         /* servos */
 215         PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
 216                                  TIMER1_PRESCALER_DIV_256);
 217         /* 1 et 2 ko XXX */
 218         PWM_NG_INIT16(&arm.servo1, 3, A, 10, PWM_NG_MODE_NORMAL,
 219                       NULL, 0);
 220         PWM_NG_INIT16(&arm.servo2, 3, B, 10, PWM_NG_MODE_NORMAL,
 221                       NULL, 0);
 222         PWM_NG_INIT16(&arm.servo3, 3, C, 10, PWM_NG_MODE_NORMAL,
 223                       NULL, 0);
 224         PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10,
 225                                  TIMER1_PRESCALER_DIV_256);
 226         PWM_NG_INIT16(&arm.servo4, 5, A, 10, PWM_NG_MODE_NORMAL,
 227                       NULL, 0);
 228         PWM_NG_INIT16(&arm.servo5, 5, B, 10, PWM_NG_MODE_NORMAL,
 229                       NULL, 0);
 230         PWM_NG_INIT16(&arm.servo6, 5, C, 10, PWM_NG_MODE_NORMAL,
 231                       NULL, 0);
 232
 233
 234         /* CS EVENT */
 235         scheduler_add_periodical_event_priority(do_cs, NULL,
 236                                                 5000L / SCHEDULER_UNIT,
 237                                                 CS_PRIO);
 238         /* TIME */
 239         time_init(TIME_PRIO);
 240
 241         printf_P(PSTR("Coucou\r\n"));
 242         sei();
 243
 244         rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
 245         snprintf(prompt, sizeof(prompt), "atm2560 > ");
 246         rdline_newline(&rdl, prompt);
 247
 248
 249         while (1) {
 250                 c = uart_recv_nowait(3);
 251                 if (c == -1)
 252                         continue;
 253                 ret = rdline_char_in(&rdl, c);
 254                 if (ret != 2 && ret != 0) {
 255                         buffer = rdline_get_buffer(&rdl);
 256                         history = rdline_get_history_item(&rdl, 0);
 257                         if (history) {
 258                                 same = !memcmp(buffer, history, strlen(history)) &&
 259                                         buffer[strlen(history)] == '\n';
 260                         }
 261                         else
 262                                 same = 0;
 263                         if (strlen(buffer) > 1 && !same)
 264                                 rdline_add_history(&rdl, buffer);
 265                         rdline_newline(&rdl, prompt);
 266                 }
 267         }
 268
 269         return 0;
 270 }