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/asserv.cc | 387 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 387 insertions(+) create mode 100644 i/chuck/src/asserv/asserv.cc (limited to 'i/chuck/src/asserv/asserv.cc') 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); +} + -- cgit v1.2.3