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/Makefile.defs | 5 + i/chuck/src/asserv/asserv.cc | 387 ++++++++++++++++++++++++++++++++++++++ i/chuck/src/asserv/asserv.hh | 141 ++++++++++++++ i/chuck/src/asserv/test_asserv.cc | 197 +++++++++++++++++++ 4 files changed, 730 insertions(+) create mode 100644 i/chuck/src/asserv/Makefile.defs create mode 100644 i/chuck/src/asserv/asserv.cc create mode 100644 i/chuck/src/asserv/asserv.hh create mode 100644 i/chuck/src/asserv/test_asserv.cc (limited to 'i/chuck/src/asserv') diff --git a/i/chuck/src/asserv/Makefile.defs b/i/chuck/src/asserv/Makefile.defs new file mode 100644 index 0000000..22ffdb2 --- /dev/null +++ b/i/chuck/src/asserv/Makefile.defs @@ -0,0 +1,5 @@ +PROGRAMS += test_asserv + +asserv_OBJECTS = asserv.o $(proto_OBJECTS) + +test_asserv_OBJECTS = test_asserv.o $(asserv_OBJECTS) $(tester_OBJECTS) diff --git a/i/chuck/src/asserv/asserv.cc b/i/chuck/src/asserv/asserv.cc new file mode 100644 index 0000000..e196803 --- /dev/null +++ b/i/chuck/src/asserv/asserv.cc @@ -0,0 +1,387 @@ +// asserv.cc +// marvin - programme du robot 2006. {{{ +// +// Copyright (C) 2006 Nicolas Schodet +// +// Robot APB Team/Efrei 2006. +// 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 "config/config.hh" + +#include + +/// Constructor. +Asserv::Asserv (Asserv::Receiver &receiver) + : proto_ (*this), receiver_ (receiver) +{ + // Get Config + Config &config = Config::getInstance (); + intervalCounterStat_ = config.get ("asserv.counter_stat", 0); + intervalPos_ = config.get ("asserv.pos_report"); + intervalSpeedStat_ = config.get ("asserv.speed_stat", 0); + intervalPosStat_ = config.get ("asserv.pos_stat", 0); + intervalPwmStat_ = config.get ("asserv.pwm_stat", 0); + intervalTimerStat_ = config.get ("asserv.timer_stat", 0); + intervalInPort_ = config.get ("asserv.in_port_report"); + footing_ = config.get ("asserv.footing"); + tAccel_ = config.get ("asserv.t_accel"); + aAccel_ = config.get ("asserv.a_accel"); + tMaxSpeed_ = config.get ("asserv.t_max_speed"); + aMaxSpeed_ = config.get ("asserv.a_max_speed"); + tMaxSpeedSlow_ = config.get ("asserv.t_max_speed_slow"); + aMaxSpeedSlow_ = config.get ("asserv.a_max_speed_slow"); + tkp_ = config.get ("asserv.tkp"); + tki_ = config.get ("asserv.tki"); + tkd_ = config.get ("asserv.tkd"); + akp_ = config.get ("asserv.akp"); + aki_ = config.get ("asserv.aki"); + akd_ = config.get ("asserv.akd"); + esat_ = config.get ("asserv.esat"); + isat_ = config.get ("asserv.isat"); + lInvertPwm_ = config.get ("asserv.l_invert_pwm"); + rInvertPwm_ = config.get ("asserv.r_invert_pwm"); + stepPerMm_ = config.get ("asserv.step_per_mm"); + tty_ = config.get ("asserv.tty"); + proto_.open (tty_); + reset (); +} + +/// Essaie de purger la liste d'émission et indique si elle est vide. +bool +Asserv::sync (void) +{ + return proto_.sync (); +} + +/// Attend que toute les émissions soit terminées. +bool +Asserv::wait (int timeout/*-1*/) +{ + return proto_.wait (timeout); +} + +/// Récupère le File Descriptor +int +Asserv::getFd (void) const +{ + return proto_.getFd (); +} + +/// Reset the board and send parameters. +void +Asserv::reset (void) +{ + proto_.send ('z'); + setIntervalCounterStat (intervalCounterStat_); + setIntervalPos (intervalPos_); + setIntervalSpeedStat (intervalSpeedStat_); + setIntervalPosStat (intervalPosStat_); + setIntervalPwmStat (intervalPwmStat_); + setIntervalTimerStat (intervalTimerStat_); + setIntervalInPort (intervalInPort_); + setFooting (footing_); + setAccel (tAccel_, aAccel_); + setMaxSpeed (tMaxSpeed_, aMaxSpeed_, tMaxSpeedSlow_, aMaxSpeedSlow_); + setCoef (tkp_, tki_, tkd_, akp_, akd_, akd_, esat_, isat_); + setPwmDir (lInvertPwm_, rInvertPwm_); +} + +/// Set PWM. +void +Asserv::pwm (int l, int r) +{ + proto_.send ('w', "ww", l, r); +} + +/// Position consign offset. +void +Asserv::offset (double t, double a) +{ + proto_.send ('c', "ww", mmToStep (t), mmToStep (a)); +} + +/// Set speed. +void +Asserv::speed (int t, int a) +{ + proto_.send ('s', "bb", t, a); +} + +/// Speed controlled position consign offset. +void +Asserv::speedTo (double t, double a, int seq) +{ + proto_.send ('s', "ddb", mmToStep (t), mmToStep (a), seq); +} + +/// Speed controlled angle consign offset. +void +Asserv::speedAngle (double a, int seq) +{ + proto_.send ('s', "ddb", 0, radToStep (a), seq); +} + +/// Find a hole. +void +Asserv::findHole (int seq) +{ + proto_.send ('h', "b", seq); +} + +/// Acknoledge. +void +Asserv::ack (int seq) +{ + proto_.send ('a', "b", seq); +} + +/// Change counter stat interval. +void +Asserv::setIntervalCounterStat (int i) +{ + intervalCounterStat_ = i; + proto_.send ('C', "b", i); +} + +/// Change position report interval. +void +Asserv::setIntervalPos (int i) +{ + intervalPos_ = i; + proto_.send ('X', "b", i); +} + +/// Change speed stat interval. +void +Asserv::setIntervalSpeedStat (int i) +{ + intervalSpeedStat_ = i; + proto_.send ('S', "b", i); +} + +/// Change position control stat interval. +void +Asserv::setIntervalPosStat (int i) +{ + intervalPosStat_ = i; + proto_.send ('P', "b", i); +} + +/// Change pwm stat interval. +void +Asserv::setIntervalPwmStat (int i) +{ + intervalPwmStat_ = i; + proto_.send ('W', "b", i); +} + +/// Change timer stat interval. +void +Asserv::setIntervalTimerStat (int i) +{ + intervalTimerStat_ = i; + proto_.send ('T', "b", i); +} + +/// Change input port report interval. +void +Asserv::setIntervalInPort (int i) +{ + intervalInPort_ = i; + proto_.send ('I', "b", i); +} + +/// Set current position. +void +Asserv::setPos (double x, double y, double a) +{ + setXPos (x); + setYPos (y); + setAngle (a); +} + +/// Set current x position. +void +Asserv::setXPos (double x) +{ + proto_.send ('p', "bd", 'X', mmToStep (x)); +} + +/// Set current y position. +void +Asserv::setYPos (double y) +{ + proto_.send ('p', "bd", 'Y', mmToStep (y)); +} + +/// Set current angle. +void +Asserv::setAngle (double a) +{ + proto_.send ('p', "bd", 'A', radToAvr (a)); +} + +/// Set footing. +void +Asserv::setFooting (int f) +{ + footing_ = f; + proto_.send ('p', "bw", 'f', f); +} + +/// Set acceleration. +void +Asserv::setAccel (int t, int a) +{ + tAccel_ = t; + aAccel_ = a; + proto_.send ('p', "bww", 'a', t, a); +} + +/// Set maximum speed for automatic movements. +void +Asserv::setMaxSpeed (int t, int a, int ts, int as) +{ + tMaxSpeed_ = t; + aMaxSpeed_ = a; + tMaxSpeedSlow_ = ts; + aMaxSpeedSlow_ = as; + proto_.send ('p', "bbbbb", 's', t, a, ts, as); +} + +/// Set motor control coeficients. +void +Asserv::setCoef (int tkp, int tki, int tkd, int akp, int aki, int akd, + int esat, int isat) +{ + tkp_ = tkp; + tki_ = tki; + tkd_ = tkd; + akp_ = akp; + aki_ = aki; + akd_ = akd; + esat_ = esat; + isat_ = isat; + proto_.send ('p', "bww", 'p', tkp, akp); + proto_.send ('p', "bww", 'i', tki, aki); + proto_.send ('p', "bww", 'd', tkd, akd); + proto_.send ('p', "bw", 'E', esat); + proto_.send ('p', "bw", 'I', isat); +} + +/// Set PWM direction. +void +Asserv::setPwmDir (bool invertL, bool invertR) +{ + lInvertPwm_ = invertL; + rInvertPwm_ = invertR; + proto_.send ('p', "bbb", 'w', invertL ? 1 : 0, invertR ? 1 : 0); +} + +/// Store to eeprom. +void +Asserv::storeParams (void) +{ + proto_.send ('p', "bb", 'E', 1); +} + +/// Clear eeprom. +void +Asserv::clearParams (void) +{ + proto_.send ('p', "bb", 'E', 0); +} + +/// Implémentation du proto::Receiver. +void +Asserv::receive (char command, const Proto::Frame &frame) +{ + int seq, l, r, x, y, a, t, te, ti, ae, ai, port; + switch (command) + { + case 'A': + if (proto_.decode (frame, "b", seq)) + receiver_.receiveAck (seq); + break; + case 'C': + if (proto_.decode (frame, "ww", l, r)) + receiver_.receiveCounterStat (l, r); + break; + case 'X': + if (proto_.decode (frame, "DDD", x, y, a)) + receiver_.receivePos (stepToMm (x / 256), stepToMm (y / 256), avrToRad (a)); + break; + case 'S': + if (proto_.decode (frame, "BB", t, a)) + receiver_.receiveSpeedStat (t, a); + break; + case 'P': + if (proto_.decode (frame, "WWWW", te, ti, ae, ai)) + receiver_.receivePosStat (te, ti, ae, ai); + break; + case 'W': + if (proto_.decode (frame, "WW", l, r)) + receiver_.receivePwmStat (l, r); + break; + case 'T': + // TODO (si on a le temps). + break; + case 'I': + if (proto_.decode (frame, "w", port)) + receiver_.receiveInPort (port); + break; + } +} + +/// Convert mm to steps. +int +Asserv::mmToStep (double mm) const +{ + return static_cast (mm * stepPerMm_); +} + +/// Convert steps to mm. +double +Asserv::stepToMm (int step) const +{ + return static_cast (step) / stepPerMm_; +} + +/// Convert rad to avr angles. +int +Asserv::radToAvr (double a) const +{ + return static_cast (a * M_1_PI * 0.5 * (1 << 24)) & 0xffffff; +} + +/// Convert avr angles to rad. +double +Asserv::avrToRad (int a) const +{ + return static_cast (a) * 2 * M_PI / (1 << 24); +} + +/// Convert rad to steps. +int +Asserv::radToStep (double a) const +{ + return static_cast (static_cast (footing_) * a * 0.5); +} + diff --git a/i/chuck/src/asserv/asserv.hh b/i/chuck/src/asserv/asserv.hh new file mode 100644 index 0000000..ddbc104 --- /dev/null +++ b/i/chuck/src/asserv/asserv.hh @@ -0,0 +1,141 @@ +#ifndef asserv_hh +#define asserv_hh +// asserv.hh +// marvin - programme du robot 2006. {{{ +// +// Copyright (C) 2006 Nicolas Schodet +// +// Robot APB Team/Efrei 2006. +// 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 "proto/proto.hh" + +/// Dialog class with the motor control board. +/// Units: +/// - step: one step distance. +/// - period: one motor control sampling period. +class Asserv : public Proto::Receiver +{ + public: + /// Asserv clients must implement Receiver. + class Receiver + { + public: + virtual ~Receiver (void) { } + virtual void receiveAck (int seq) = 0; + virtual void receiveCounterStat (int l, int r) = 0; + virtual void receivePos (double x, double y, double a) = 0; + virtual void receiveSpeedStat (int t, int a) = 0; + virtual void receivePosStat (int te, int ti, int ae, int ai) = 0; + virtual void receivePwmStat (int l, int r) = 0; + virtual void receiveTimerStat (const int *t, int tn) = 0; + virtual void receiveInPort (unsigned int port) = 0; + }; + private: + Proto proto_; + std::string tty_; + Receiver &receiver_; + int intervalCounterStat_, intervalPos_, intervalSpeedStat_, + intervalPosStat_, intervalPwmStat_, intervalTimerStat_, + intervalInPort_; + int footing_; + int tAccel_, aAccel_; + int tMaxSpeed_, aMaxSpeed_, tMaxSpeedSlow_, aMaxSpeedSlow_; + int tkp_, tki_, tkd_, akp_, aki_, akd_, esat_, isat_; + bool lInvertPwm_, rInvertPwm_; + double stepPerMm_; + public: + /// Constructor. + Asserv (Asserv::Receiver &receiver); + /// Essaie de purger la liste d'émission et indique si elle est vide. + bool sync (void); + /// Attend que toute les émissions soit terminées. + bool wait (int timeout = -1); + /// Récupère le File Descriptor + int getFd (void) const; + /// Reset the board and send parameters. + void reset (void); + /// Set PWM. + void pwm (int l, int r); + /// Position consign offset. + void offset (double t, double a); + /// Set speed. + void speed (int t, int a); + /// Speed controlled position consign offset. + void speedTo (double t, double a, int seq); + /// Speed controlled angle consign offset. + void speedAngle (double a, int seq); + /// Find a hole. + void findHole (int seq); + /// Acknoledge. + void ack (int seq); + /// Change counter stat interval. + void setIntervalCounterStat (int i); + /// Change position report interval. + void setIntervalPos (int i); + /// Change speed stat interval. + void setIntervalSpeedStat (int i); + /// Change position control stat interval. + void setIntervalPosStat (int i); + /// Change pwm stat interval. + void setIntervalPwmStat (int i); + /// Change timer stat interval. + void setIntervalTimerStat (int i); + /// Change input port report interval. + void setIntervalInPort (int i); + /// Set current position. + void setPos (double x, double y, double a); + /// Set current x position. + void setXPos (double x); + /// Set current y position. + void setYPos (double y); + /// Set current angle. + void setAngle (double a); + /// Set footing. + void setFooting (int f); + /// Set acceleration. + void setAccel (int t, int a); + /// Set maximum speed for automatic movements. + void setMaxSpeed (int t, int a, int ts, int as); + /// Set motor control coeficients. + void setCoef (int tkp, int tki, int tkd, int akp, int aki, int akd, + int esat, int isat); + /// Set PWM direction. + void setPwmDir (bool invertL, bool invertR); + /// Store to eeprom. + void storeParams (void); + /// Clear eeprom. + void clearParams (void); + /// Implémentation du proto::Receiver. + void receive (char command, const Proto::Frame &frame); + private: + /// Convert mm to steps. + int mmToStep (double mm) const; + /// Convert steps to mm. + double stepToMm (int step) const; + /// Convert rad to avr angles. + int radToAvr (double a) const; + /// Convert avr angles to rad. + double avrToRad (int a) const; + /// Convert rad to steps. + int radToStep (double a) const; +}; + +#endif // asserv_hh 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