summaryrefslogtreecommitdiff
path: root/i/chuck/src/asserv/asserv.cc
diff options
context:
space:
mode:
authorbecquet2007-05-10 18:49:20 +0000
committerbecquet2007-05-10 18:49:20 +0000
commit8f486613be58ced269db1d437e560c16558604e8 (patch)
tree41e94b2122a118cb06abf6fc2a0038cd1dfbec4a /i/chuck/src/asserv/asserv.cc
parent4daa2c76c2a028e4b2c8ab379e7d1e0f535a0a31 (diff)
Création de chuck, le programme du robot 2007.
Diffstat (limited to 'i/chuck/src/asserv/asserv.cc')
-rw-r--r--i/chuck/src/asserv/asserv.cc387
1 files changed, 387 insertions, 0 deletions
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 <math.h>
+
+/// Constructor.
+Asserv::Asserv (Asserv::Receiver &receiver)
+ : proto_ (*this), receiver_ (receiver)
+{
+ // Get Config
+ Config &config = Config::getInstance ();
+ intervalCounterStat_ = config.get<int> ("asserv.counter_stat", 0);
+ intervalPos_ = config.get<int> ("asserv.pos_report");
+ intervalSpeedStat_ = config.get<int> ("asserv.speed_stat", 0);
+ intervalPosStat_ = config.get<int> ("asserv.pos_stat", 0);
+ intervalPwmStat_ = config.get<int> ("asserv.pwm_stat", 0);
+ intervalTimerStat_ = config.get<int> ("asserv.timer_stat", 0);
+ intervalInPort_ = config.get<int> ("asserv.in_port_report");
+ footing_ = config.get<int> ("asserv.footing");
+ tAccel_ = config.get<int> ("asserv.t_accel");
+ aAccel_ = config.get<int> ("asserv.a_accel");
+ tMaxSpeed_ = config.get<int> ("asserv.t_max_speed");
+ aMaxSpeed_ = config.get<int> ("asserv.a_max_speed");
+ tMaxSpeedSlow_ = config.get<int> ("asserv.t_max_speed_slow");
+ aMaxSpeedSlow_ = config.get<int> ("asserv.a_max_speed_slow");
+ tkp_ = config.get<int> ("asserv.tkp");
+ tki_ = config.get<int> ("asserv.tki");
+ tkd_ = config.get<int> ("asserv.tkd");
+ akp_ = config.get<int> ("asserv.akp");
+ aki_ = config.get<int> ("asserv.aki");
+ akd_ = config.get<int> ("asserv.akd");
+ esat_ = config.get<int> ("asserv.esat");
+ isat_ = config.get<int> ("asserv.isat");
+ lInvertPwm_ = config.get<bool> ("asserv.l_invert_pwm");
+ rInvertPwm_ = config.get<bool> ("asserv.r_invert_pwm");
+ stepPerMm_ = config.get<double> ("asserv.step_per_mm");
+ tty_ = config.get<std::string> ("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<int> (mm * stepPerMm_);
+}
+
+/// Convert steps to mm.
+double
+Asserv::stepToMm (int step) const
+{
+ return static_cast<double> (step) / stepPerMm_;
+}
+
+/// Convert rad to avr angles.
+int
+Asserv::radToAvr (double a) const
+{
+ return static_cast<int> (a * M_1_PI * 0.5 * (1 << 24)) & 0xffffff;
+}
+
+/// Convert avr angles to rad.
+double
+Asserv::avrToRad (int a) const
+{
+ return static_cast<double> (a) * 2 * M_PI / (1 << 24);
+}
+
+/// Convert rad to steps.
+int
+Asserv::radToStep (double a) const
+{
+ return static_cast<int> (static_cast<double> (footing_) * a * 0.5);
+}
+