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.2 2009-02-20 21:10:01 zer0 Exp $
27 #include <aversive/pgmspace.h>
28 #include <aversive/wait.h>
36 #include <scheduler.h>
40 #include <control_system_manager.h>
44 /* ---- with 16 Mhz quartz
49 * BOOTZ 1-0 : 01 (4K bootloader)
50 * BOOTRST : 0 (reset on bootloader)
53 /* for cmdline interface */
55 char prompt[RDLINE_PROMPT_SIZE];
56 extern parse_pgm_ctx_t main_ctx[];
62 /******** For cmdline. See in commands.c for the list of commands. */
63 static void write_char(char c)
69 valid_buffer(const char * buf, uint8_t size)
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"));
82 complete_buffer(const char * buf, char * dstbuf, uint8_t dstsize,
85 return complete(main_ctx, buf, state, dstbuf, dstsize);
89 /***********************/
91 void do_led_blink(void * dummy)
93 #if 1 /* simple blink */
105 /* called every 5 ms */
106 static void do_cs(void * dummy)
108 if (arm.flags & CS_ON)
109 cs_manage(&arm.cs_mot);
112 static void main_timer_interrupt(void)
114 static uint8_t cpt = 0;
119 if ((cpt & 0x3) == 0)
120 scheduler_interrupt();
123 /* sending "pop" on uart0 resets the robot */
124 static void emergency(char c)
126 static uint8_t i = 0;
128 if ((i == 0 && c == 'p') ||
129 (i == 1 && c == 'o') ||
130 (i == 2 && c == 'p'))
132 else if ( !(i == 1 && c == 'p') )
141 const char *history, *buffer;
142 int8_t ret, same = 0;
154 memset(&arm, 0, sizeof(struct arm));
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);
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 */
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); */
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);
185 fdevopen(uart3_dev_send, uart3_dev_recv);
186 uart_register_rx_event(3, emergency);
190 timer0_register_OV_intr(main_timer_interrupt);
194 scheduler_add_periodical_event_priority(do_led_blink, NULL,
195 100000L / SCHEDULER_UNIT,
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);
204 PWM_NG_INIT8_B(&arm.pwm1_2A, 2, A, 8, PWM_NG_MODE_SIGNED,
206 PWM_NG_INIT16(&arm.pwm2_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
208 PWM_NG_INIT16(&arm.pwm3_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
210 PWM_NG_INIT16(&arm.pwm4_1C, 1, C, 10, PWM_NG_MODE_SIGNED,
215 PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
216 TIMER1_PRESCALER_DIV_256);
218 PWM_NG_INIT16(&arm.servo1, 3, A, 10, PWM_NG_MODE_NORMAL,
220 PWM_NG_INIT16(&arm.servo2, 3, B, 10, PWM_NG_MODE_NORMAL,
222 PWM_NG_INIT16(&arm.servo3, 3, C, 10, PWM_NG_MODE_NORMAL,
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,
228 PWM_NG_INIT16(&arm.servo5, 5, B, 10, PWM_NG_MODE_NORMAL,
230 PWM_NG_INIT16(&arm.servo6, 5, C, 10, PWM_NG_MODE_NORMAL,
235 scheduler_add_periodical_event_priority(do_cs, NULL,
236 5000L / SCHEDULER_UNIT,
239 time_init(TIME_PRIO);
241 printf_P(PSTR("Coucou\r\n"));
244 rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
245 snprintf(prompt, sizeof(prompt), "atm2560 > ");
246 rdline_newline(&rdl, prompt);
250 c = uart_recv_nowait(3);
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);
258 same = !memcmp(buffer, history, strlen(history)) &&
259 buffer[strlen(history)] == '\n';
263 if (strlen(buffer) > 1 && !same)
264 rdline_add_history(&rdl, buffer);
265 rdline_newline(&rdl, prompt);