From 89fa4dc287106fa0be4bd35cc68af6b1d59e56cb Mon Sep 17 00:00:00 2001 From: Maxime Hadjinlian Date: Wed, 8 May 2013 19:29:09 +0200 Subject: digital/io-hub/src/apbirthday: add servos class --- digital/io-hub/src/apbirthday/Makefile | 1 + digital/io-hub/src/apbirthday/cannon.cc | 1 + digital/io-hub/src/apbirthday/cannon.hh | 6 +- digital/io-hub/src/apbirthday/hardware.hh | 2 + digital/io-hub/src/apbirthday/hardware.stm32.cc | 5 +- digital/io-hub/src/apbirthday/robot.cc | 7 ++ digital/io-hub/src/common-cc/servo.hh | 42 ++++++++++++ digital/io-hub/src/common-cc/servo.host.cc | 39 +++++++++++ digital/io-hub/src/common-cc/servo.stm32.cc | 89 +++++++++++++++++++++++++ 9 files changed, 188 insertions(+), 4 deletions(-) create mode 100644 digital/io-hub/src/common-cc/servo.hh create mode 100644 digital/io-hub/src/common-cc/servo.host.cc create mode 100644 digital/io-hub/src/common-cc/servo.stm32.cc (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 215c71bc..69c7027b 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -7,6 +7,7 @@ apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ i2c_queue.cc asserv.cc mimot.cc lcd.cc beacon.cc \ potentiometer.host.cc potentiometer.stm32.cc \ pressure.cc chrono.host.cc chrono.stm32.cc debounce.cc \ + servo.host.cc servo.stm32.cc \ radar.cc radar_2013.cc obstacles.cc path.cc path_2013.cc strat.cc \ outputs.cc \ top.cc init.cc move.cc candles.cc cannon.cc drinks.cc plate.cc \ diff --git a/digital/io-hub/src/apbirthday/cannon.cc b/digital/io-hub/src/apbirthday/cannon.cc index d06412ef..3b733d19 100644 --- a/digital/io-hub/src/apbirthday/cannon.cc +++ b/digital/io-hub/src/apbirthday/cannon.cc @@ -45,6 +45,7 @@ inline void Cannon::blower_off () inline void Cannon::set_servo_pos (int pos) { // Switch the servo to BLOCK, POS1 or POS2 + robot->hardware.servos.set_position (Servo::SERVO_CHERRY, pos); } inline void Cannon::set_router_state (int state) diff --git a/digital/io-hub/src/apbirthday/cannon.hh b/digital/io-hub/src/apbirthday/cannon.hh index cd7a2969..a8c0ce26 100644 --- a/digital/io-hub/src/apbirthday/cannon.hh +++ b/digital/io-hub/src/apbirthday/cannon.hh @@ -31,9 +31,9 @@ class Cannon Cannon (); enum ServoPos { - BLOCK, - POS1, - POS2 + BLOCK = 0x0800, + POS1 = 0x1380, + POS2 = 0x0a00 }; enum StateRouter { diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index 33d10c0c..d53d3ec8 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -33,6 +33,7 @@ #else # include "ucoolib/arch/host/host_stream.hh" #endif +#include "servo.hh" #ifdef TARGET_host # include "simu_report.host.hh" #endif @@ -101,6 +102,7 @@ struct Hardware ucoo::AdcHost adc_cake_front, adc_cake_back; ucoo::AdcHost adc_pressure; #endif + Servo servos; #ifdef TARGET_host SimuReport simu_report; #endif diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index bfca0a61..26510abc 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -64,7 +64,8 @@ Hardware::Hardware () adc_dist0 (adc, 0), adc_dist1 (adc, 1), adc_dist2 (adc, 2), adc_dist3 (adc, 3), adc_cake_front (adc, 6), adc_cake_back (adc, 7), - adc_pressure (adc, 8) + adc_pressure (adc, 8), + servos () { rcc_peripheral_enable_clock (&RCC_AHB1ENR, RCC_AHB1ENR_IOPAEN); rcc_peripheral_enable_clock (&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN); @@ -105,6 +106,8 @@ Hardware::Hardware () GPIO10 | GPIO11); gpio_set_af (GPIOB, GPIO_AF4, GPIO10 | GPIO11); zb_i2c.enable (); + // Servos + servos.enable (); // GPIO. raw_jack.pull (ucoo::Gpio::PULL_UP); ihm_color.pull (ucoo::Gpio::PULL_UP); diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 909bbc31..99926707 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -336,6 +336,13 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size pot_regul.set_wiper (args[0], ucoo::bytes_pack (args[1], args[2]), args[3] ? true : false); break; + case c ('s', 3): + // Set servo position. + // - 1b: servo index. + // - 1h: servo position. + hardware.servos.set_position (args[0], + ucoo::bytes_pack (args[1], args[2])); + break; case c ('l', 3): // Test LCD interface, set team color. // - 3B: R, G, B. diff --git a/digital/io-hub/src/common-cc/servo.hh b/digital/io-hub/src/common-cc/servo.hh new file mode 100644 index 00000000..c55fd69e --- /dev/null +++ b/digital/io-hub/src/common-cc/servo.hh @@ -0,0 +1,42 @@ +#ifndef servo_hh +#define servo_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// 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. +// +// }}} + +class Servo +{ + public: + Servo (); + void enable (); + void set_position (int servo, int pos); + enum Servos + { + SERVO_CAKE, + SERVO_CHERRY + }; + private: + int servo_pos; +}; + +#endif // servo_hh diff --git a/digital/io-hub/src/common-cc/servo.host.cc b/digital/io-hub/src/common-cc/servo.host.cc new file mode 100644 index 00000000..9446585b --- /dev/null +++ b/digital/io-hub/src/common-cc/servo.host.cc @@ -0,0 +1,39 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// 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 "servo.hh" + +Servo::Servo () +{ +} + +void +Servo::enable () +{ +} + +void +Servo::set_position(int servo, int pos) +{ +} + diff --git a/digital/io-hub/src/common-cc/servo.stm32.cc b/digital/io-hub/src/common-cc/servo.stm32.cc new file mode 100644 index 00000000..016128ed --- /dev/null +++ b/digital/io-hub/src/common-cc/servo.stm32.cc @@ -0,0 +1,89 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// 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 "servo.hh" + +#include +#include +#include + +Servo::Servo () +{ +} + +void +Servo::enable () +{ + rcc_peripheral_enable_clock (&RCC_APB2ENR, RCC_APB2ENR_TIM8EN); + timer_reset(TIM8); + + timer_set_mode(TIM8, + TIM_CR1_CKD_CK_INT, + TIM_CR1_CMS_EDGE, + TIM_CR1_DIR_UP); + + timer_set_prescaler(TIM8, 40); + // 66Mhz which seems to be a pretty good fit for servos + timer_set_period(TIM8, 43290); + timer_set_repetition_counter(TIM8, 0); + timer_continuous_mode(TIM8); + + // Cake servo 1 + gpio_mode_setup(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO7); + gpio_set_af(GPIOC, GPIO_AF3, GPIO7); + + // Cherry servo 2 + gpio_mode_setup(GPIOC, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8); + gpio_set_af(GPIOC, GPIO_AF3, GPIO8); + + // Cake servo 1 + timer_disable_oc_output(TIM8, TIM_OC2); + timer_set_oc_mode(TIM8, TIM_OC2, TIM_OCM_PWM1); + timer_enable_oc_output(TIM8, TIM_OC2); + + // Cherry servo 2 + timer_disable_oc_output(TIM8, TIM_OC3); + timer_set_oc_mode(TIM8, TIM_OC3, TIM_OCM_PWM1); + timer_enable_oc_output(TIM8, TIM_OC3); + + timer_enable_counter(TIM8); + timer_enable_break_main_output(TIM8); + + // Set null position, so the servo doesn't move. + set_position(SERVO_CAKE, 0); + set_position(SERVO_CHERRY, 0); +} + +void +Servo::set_position(int servo, int pos) +{ + if (servo == SERVO_CAKE) + { + timer_set_oc_value(TIM8, TIM_OC2, pos); + } + else if (servo == SERVO_CHERRY) + { + timer_set_oc_value(TIM8, TIM_OC3, pos); + } +} + -- cgit v1.2.3