From b1f14380727106642388a6d32baa3a4103a03a9b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 3 Oct 2011 21:03:58 +0200 Subject: digital/avr/modules/motor: add new output module --- .../avr/modules/motor/output/test/test_output.c | 154 +++++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 digital/avr/modules/motor/output/test/test_output.c (limited to 'digital/avr/modules/motor/output/test/test_output.c') diff --git a/digital/avr/modules/motor/output/test/test_output.c b/digital/avr/modules/motor/output/test/test_output.c new file mode 100644 index 00000000..f38c7383 --- /dev/null +++ b/digital/avr/modules/motor/output/test/test_output.c @@ -0,0 +1,154 @@ +/* test_output.c */ +/* motor - Motor control module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" + +#include "modules/utils/utils.h" +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" + +#include "modules/motor/output/output.h" + +#include "io.h" + +output_t output[OUTPUT_NB]; + +int16_t target[OUTPUT_NB]; +int16_t current[OUTPUT_NB]; + +uint16_t output_speed_cpt = 1, output_speed = 250; +uint16_t output_stat_cpt, output_stat; + +int +main (void) +{ + uint8_t i; + for (i = 0; i < OUTPUT_NB; i++) + { + output[i].min = 0x10; + output[i].max = OUTPUT_MAX; + output_init (i, &output[i]); + } + uart0_init (); + proto_send0 ('z'); + sei (); + while (1) + { + utils_delay_ms (4); + if (output_speed && !--output_speed_cpt) + { + for (i = 0; i < OUTPUT_NB; i++) + { + if (target[i] < current[i]) + current[i]--; + else if (target[i] > output[i].cur) + current[i]++; + output_set (&output[i], current[i]); + } + output_speed_cpt = output_speed; + } + if (output_stat && !--output_stat_cpt) + { + proto_send2w ('W', output[0].cur, output[1].cur); + output_stat_cpt = output_stat; + } + while (uart0_poll ()) + proto_accept (uart0_getc ()); + } +} + +/** Handle incoming messages. */ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ + uint8_t i; + uint8_t ok = 1; + uint8_t index = args[0]; + int16_t value = v8_to_v16 (args[1], args[2]); +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* Reset. */ + utils_reset (); + break; + case c ('w', 0): + /* Set zero value. */ + for (i = 0; i < OUTPUT_NB; i++) + { + output_set (&output[index], 0); + current[index] = target[index] = 0; + } + case c ('w', 3): + /* Set direct value. + * - b: output index. + * - w: output value. */ + if (index < OUTPUT_NB) + { + output_set (&output[index], value); + target[index] = current[index]; + } + else + ok = 0; + break; + case c ('t', 3): + /* Set target. + * - b: output index. + * - w: output target. */ + if (index < OUTPUT_NB) + { + current[index] = output[index].cur; + target[index] = value; + } + else + ok = 0; + break; + case c ('s', 2): + /* Set targeting speed. + * - w: loops between update. */ + output_speed_cpt = output_speed = v8_to_v16 (args[0], args[1]); + break; + case c ('r', 2): + /* Set reverse flag. + * - b: output index. + * - b: reverse flag. */ + if (index < OUTPUT_NB) + output_set_reverse (&output[index], args[1]); + else + ok = 0; + break; + case c ('W', 2): + /* Output stats. */ + output_stat_cpt = output_stat = v8_to_v16 (args[0], args[1]); + break; + default: + ok = 0; + break; + } + if (ok) + proto_send (cmd, size, args); + else + proto_send0 ('?'); +#undef c +} -- cgit v1.2.3