From 8f486613be58ced269db1d437e560c16558604e8 Mon Sep 17 00:00:00 2001 From: becquet Date: Thu, 10 May 2007 18:49:20 +0000 Subject: Création de chuck, le programme du robot 2007. --- i/chuck/src/asserv/test_asserv.cc | 197 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 197 insertions(+) create mode 100644 i/chuck/src/asserv/test_asserv.cc (limited to 'i/chuck/src/asserv/test_asserv.cc') diff --git a/i/chuck/src/asserv/test_asserv.cc b/i/chuck/src/asserv/test_asserv.cc new file mode 100644 index 0000000..2796e33 --- /dev/null +++ b/i/chuck/src/asserv/test_asserv.cc @@ -0,0 +1,197 @@ +// test_asserv.cc +// marvin - programme du robot 2006. {{{ +// +// Copyright (C) 2006 Nicolas Schodet +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// 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 "asserv.hh" +#include "tester/tester.hh" +#include "timer/timer.hh" + +#include +#include +#include + +class TestAsserv : public Tester, public Asserv::Receiver +{ + private: + Asserv asserv_; + int seq_; + bool ok_; + public: + /// Constructor. + TestAsserv (int argc, char **argv) + : Tester (argc, argv), asserv_ (*this), seq_ (0), ok_ (true) + { } + /// Next seq number. + inline void nextSeq (void) + { + seq_++; + if (seq_ > 100) + seq_ = 1; + } + /// Speed controlled position consign offset, handle seq. + void speedTo (double t, double a) + { + nextSeq (); + ok_ = false; + asserv_.speedTo (t, a, seq_); + } + /// Speed controlled angle consign offset, handle seq. + void speedAngle (double a) + { + nextSeq (); + ok_ = false; + asserv_.speedAngle (a, seq_); + } + /// Find a hole, handle seq. + void findHole (void) + { + nextSeq (); + ok_ = false; + asserv_.findHole (seq_); + } + /// Wait. + void wait (int ms) + { + int t, stop; + t = Timer::getProgramTime (); + stop = t + ms; + while (t < stop) + { + asserv_.wait (stop - t); + t = Timer::getProgramTime (); + } + } + /// Called after each command called. + void postcall (void) + { + while (!asserv_.wait () || !ok_) + ; + } + /// Executed before checking/running commands. Good place to add command + /// to the interpreter. + void preRun (void) + { + Interpreter &i = getInterpreter (); + i.add ("wait", Interpreter::memFunc (*this, &TestAsserv::wait), + "wait MS\nwait for a delay in millisecond"); + i.add ("reset", Interpreter::memFunc (asserv_, &Asserv::reset), + "reset\nreset the asserv board"); + i.add ("pwm", Interpreter::memFunc (asserv_, &Asserv::pwm), + "pwm LEFT RIGHT\n" + "directly set pwm values, use with caution"); + i.add ("offset", Interpreter::memFunc (asserv_, &Asserv::offset), + "offset THETA(mm) ALPHA(mm)\n" + "add value to current position, use for pid callibration"); + i.add ("speed", Interpreter::memFunc (asserv_, &Asserv::speed), + "speed THETA ALPHA\n" + "unlimited speed consign"); + i.add ("speedTo", Interpreter::memFunc (*this, &TestAsserv::speedTo), + "speedTo THETA(mm) ANGLE(mm)\n" + "speed consign limited in distance"); + i.add ("speedAngle", + Interpreter::memFunc (*this, &TestAsserv::speedAngle), + "speedAngle ANGLE(rad)\n" + "speed consign limited in angle"); + i.add ("findHole", Interpreter::memFunc (*this, &TestAsserv::findHole), + "findHole\nfind a hole"); + i.add ("setPos", Interpreter::memFunc (asserv_, &Asserv::setPos), + "setPos X(mm) Y(mm) A(rad)\n" + "set current position"); + i.add ("setXPos", Interpreter::memFunc (asserv_, &Asserv::setYPos), + "setPos X(mm)\n" + "set current X position"); + i.add ("setYPos", Interpreter::memFunc (asserv_, &Asserv::setXPos), + "setPos Y(mm)\n" + "set current Y position"); + i.add ("setAngle", Interpreter::memFunc (asserv_, &Asserv::setAngle), + "setPos A(rad)\n" + "set current angle"); + i.add ("storeParams", + Interpreter::memFunc (asserv_, &Asserv::storeParams), + "storeParams\nstore parameters to eeprom"); + i.add ("clearParams", + Interpreter::memFunc (asserv_, &Asserv::clearParams), + "clearParams\nclear parameters stored in eeprom"); + i.add ("_postcall", + Interpreter::memFunc (*this, &TestAsserv::postcall)); + } + /// Executed after running commands. + void postRun (void) + { + asserv_.reset (); + } + void receiveAck (int seq) + { + if ((seq & 0x7f) == seq_) + { + std::cout << "ack received" << std::endl; + asserv_.ack (seq); + ok_ = true; + } + } + void receiveCounterStat (int l, int r) + { + std::cout << "C " << l << ' ' << r << std::endl; + } + void receivePos (double x, double y, double a) + { + std::cout << "X " << x << ' ' << y << ' ' << a << std::endl; + } + void receiveSpeedStat (int t, int a) + { + std::cout << "S " << t << ' ' << a << std::endl; + } + void receivePosStat (int te, int ti, int ae, int ai) + { + std::cout << "P " << te << ' ' << ti << ' ' << ae << ' ' << ai << + std::endl; + } + void receivePwmStat (int l, int r) + { + std::cout << "W " << l << ' ' << r << std::endl; + } + void receiveTimerStat (const int *t, int tn) + { + std::cout << "T "; + std::copy (t, t + tn, std::ostream_iterator (std::cout, " ")); + } + void receiveInPort (unsigned int port) + { + std::cout << "I " << std::hex << std::setw (4) << std::setfill ('0') + << port << std::dec << std::endl; + } +}; + +int +main (int argc, char **argv) +{ + try + { + TestAsserv ta (argc, argv); + ta.run (); + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + } +} -- cgit v1.2.3