AsebaMarxbot.cpp
Go to the documentation of this file.
00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2007--2012:
00004                 Stephane Magnenat <stephane at magnenat dot net>
00005                 (http://stephane.magnenat.net)
00006                 and other contributors, see authors.txt for details
00007         
00008         This program is free software: you can redistribute it and/or modify
00009         it under the terms of the GNU Lesser General Public License as published
00010         by the Free Software Foundation, version 3 of the License.
00011         
00012         This program is distributed in the hope that it will be useful,
00013         but WITHOUT ANY WARRANTY; without even the implied warranty of
00014         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015         GNU Lesser General Public License for more details.
00016         
00017         You should have received a copy of the GNU Lesser General Public License
00018         along with this program. If not, see <http://www.gnu.org/licenses/>.
00019 */
00020 
00021 #include "AsebaMarxbot.h"
00022 #include "../../common/consts.h"
00023 #include "../../vm/natives.h"
00024 #include "../../transport/buffer/vm-buffer.h"
00025 #include <set>
00026 #include <map>
00027 #include <cassert>
00028 #include <cstring>
00029 #include <algorithm>
00030 #include <iostream>
00031 #include <QString>
00032 #include <QApplication>
00033 #include <QMessageBox>
00034 #include <QDebug>
00035 
00040 // Implementation of aseba glue code
00041 
00042 // map for aseba glue code
00043 typedef std::map<AsebaVMState*, Enki::AsebaMarxbot*>  VmSocketMap;
00044 static VmSocketMap asebaSocketMaps;
00045 
00046 static AsebaNativeFunctionPointer nativeFunctions[] =
00047 {
00048         ASEBA_NATIVES_STD_FUNCTIONS,
00049 };
00050 
00051 static const AsebaNativeFunctionDescription* nativeFunctionsDescriptions[] =
00052 {
00053         ASEBA_NATIVES_STD_DESCRIPTIONS,
00054         0
00055 };
00056 
00057 extern "C" void AsebaPutVmToSleep(AsebaVMState *vm)
00058 {
00059 }
00060 
00061 extern "C" void AsebaSendBuffer(AsebaVMState *vm, const uint8* data, uint16 length)
00062 {
00063         Enki::AsebaMarxbot& marxBot = *asebaSocketMaps[vm];
00064         Dashel::Stream* stream = marxBot.stream;
00065         if (!stream)
00066                 return;
00067         
00068         // send to stream
00069         try
00070         {
00071                 uint16 temp;
00072                 temp = bswap16(length - 2);
00073                 stream->write(&temp, 2);
00074                 temp = bswap16(vm->nodeId);
00075                 stream->write(&temp, 2);
00076                 stream->write(data, length);
00077                 stream->flush();
00078 
00079                 // push to other nodes
00080                 for (size_t i = 0; i < marxBot.modules.size(); ++i)
00081                 {
00082                         Enki::AsebaMarxbot::Module& module = *(marxBot.modules[i]);
00083                         if (&(module.vm) != vm) 
00084                         {
00085                                 module.events.push_back(Enki::AsebaMarxbot::Event(vm->nodeId, data, length));
00086                                 AsebaProcessIncomingEvents(&(module.vm));
00087                         }
00088                 }
00089         }
00090         catch (Dashel::DashelException e)
00091         {
00092                 std::cerr << "Cannot write to socket: " << stream->getFailReason() << std::endl;
00093         }
00094 }
00095 
00096 extern "C" uint16 AsebaGetBuffer(AsebaVMState *vm, uint8* data, uint16 maxLength, uint16* source)
00097 {
00098         // TODO: improve this, it is rather ugly
00099         Enki::AsebaMarxbot& marxBot = *asebaSocketMaps[vm];
00100         for (size_t i = 0; i < marxBot.modules.size(); ++i)
00101         {
00102                 Enki::AsebaMarxbot::Module& module = *(marxBot.modules[i]);
00103                 if (&(module.vm) == vm)
00104                 {
00105                         if (module.events.empty())
00106                                 return 0;
00107                                 
00108                         // I do not put const here to work around a bug in MSVC 2005 implementation of std::valarray
00109                         Enki::AsebaMarxbot::Event& event = module.events.front();
00110                         *source = event.source;
00111                         size_t length = event.data.size();
00112                         length = std::min<size_t>(length, maxLength);
00113                         memcpy(data, &(event.data[0]), length);
00114                         module.events.pop_front();
00115                         return length;
00116                 }
00117         }
00118         return 0;
00119 }
00120 
00121 extern "C" AsebaVMDescription vmLeftMotorDescription;
00122 extern "C" AsebaVMDescription vmRightMotorDescription;
00123 extern "C" AsebaVMDescription vmProximitySensorsDescription;
00124 extern "C" AsebaVMDescription vmDistanceSensorsDescription;
00125 
00126 extern "C" const AsebaVMDescription* AsebaGetVMDescription(AsebaVMState *vm)
00127 {
00128         switch (vm->nodeId)
00129         {
00130                 case 1: return &vmLeftMotorDescription;
00131                 case 2: return &vmRightMotorDescription;
00132                 case 3: return &vmProximitySensorsDescription;
00133                 case 4: return &vmDistanceSensorsDescription;
00134                 default: break;
00135         }
00136         assert(false);
00137         return 0;
00138 }
00139 
00140 extern "C" const AsebaNativeFunctionDescription * const * AsebaGetNativeFunctionsDescriptions(AsebaVMState *vm)
00141 {
00142         return nativeFunctionsDescriptions;
00143 }
00144 
00145 static const AsebaLocalEventDescription localEvents[] = { { "timer", "periodic timer at 50 Hz" }, { NULL, NULL } };
00146 
00147 extern "C" const AsebaLocalEventDescription * AsebaGetLocalEventsDescriptions(AsebaVMState *vm)
00148 {
00149         return localEvents;
00150 }
00151 
00152 extern "C" void AsebaNativeFunction(AsebaVMState *vm, uint16 id)
00153 {
00154         nativeFunctions[id](vm);
00155 }
00156 
00157 extern "C" void AsebaWriteBytecode(AsebaVMState *vm)
00158 {
00159 }
00160 
00161 extern "C" void AsebaResetIntoBootloader(AsebaVMState *vm)
00162 {
00163 }
00164 
00165 extern "C" void AsebaAssert(AsebaVMState *vm, AsebaAssertReason reason)
00166 {
00167         std::cerr << "\nFatal error: ";
00168         switch (vm->nodeId)
00169         {
00170                 case 1: std::cerr << "left motor module"; break;
00171                 case 2: std::cerr << "right motor module"; break;
00172                 case 3: std::cerr << "proximity sensors module"; break;
00173                 case 4: std::cerr << "distance sensors module"; break;
00174                 default: std::cerr << "unknown module"; break;
00175         }
00176         std::cerr << " has produced exception: ";
00177         switch (reason)
00178         {
00179                 case ASEBA_ASSERT_UNKNOWN: std::cerr << "undefined"; break;
00180                 case ASEBA_ASSERT_UNKNOWN_UNARY_OPERATOR: std::cerr << "unknown unary operator"; break;
00181                 case ASEBA_ASSERT_UNKNOWN_BINARY_OPERATOR: std::cerr << "unknown binary operator"; break;
00182                 case ASEBA_ASSERT_UNKNOWN_BYTECODE: std::cerr << "unknown bytecode"; break;
00183                 case ASEBA_ASSERT_STACK_OVERFLOW: std::cerr << "stack overflow"; break;
00184                 case ASEBA_ASSERT_STACK_UNDERFLOW: std::cerr << "stack underflow"; break;
00185                 case ASEBA_ASSERT_OUT_OF_VARIABLES_BOUNDS: std::cerr << "out of variables bounds"; break;
00186                 case ASEBA_ASSERT_OUT_OF_BYTECODE_BOUNDS: std::cerr << "out of bytecode bounds"; break;
00187                 case ASEBA_ASSERT_STEP_OUT_OF_RUN: std::cerr << "step out of run"; break;
00188                 case ASEBA_ASSERT_BREAKPOINT_OUT_OF_BYTECODE_BOUNDS: std::cerr << "breakpoint out of bytecode bounds"; break;
00189                 case ASEBA_ASSERT_EMIT_BUFFER_TOO_LONG: std::cerr << "tried to emit a buffer too long"; break;
00190                 default: std::cerr << "unknown exception"; break;
00191         }
00192         std::cerr << ".\npc = " << vm->pc << ", sp = " << vm->sp;
00193         std::cerr << "\nResetting VM" << std::endl;
00194         assert(false);
00195         AsebaVMInit(vm);
00196 }
00197 
00198 namespace Enki
00199 {
00200         using namespace Dashel;
00201         
00202         int AsebaMarxbot::marxbotNumber = 0;
00203         
00204         AsebaMarxbot::Module::Module()
00205         {
00206                 bytecode.resize(512);
00207                 vm.bytecode = &bytecode[0];
00208                 vm.bytecodeSize = bytecode.size();
00209                 
00210                 stack.resize(64);
00211                 vm.stack = &stack[0];
00212                 vm.stackSize = stack.size();
00213         }
00214         
00215         AsebaMarxbot::AsebaMarxbot() :
00216                 stream(0)
00217         {
00218                 // setup modules specific data
00219                 leftMotor.vm.nodeId = 1;
00220                 leftMotorVariables.id = 1;
00221                 leftMotor.vm.variables = reinterpret_cast<sint16 *>(&leftMotorVariables);
00222                 leftMotor.vm.variablesSize = sizeof(leftMotorVariables) / sizeof(sint16);
00223                 modules.push_back(&leftMotor);
00224                 
00225                 rightMotor.vm.nodeId = 2;
00226                 rightMotorVariables.id = 2;
00227                 rightMotor.vm.variables = reinterpret_cast<sint16 *>(&rightMotorVariables);
00228                 rightMotor.vm.variablesSize = sizeof(rightMotorVariables) / sizeof(sint16);
00229                 modules.push_back(&rightMotor);
00230                 
00231                 proximitySensors.vm.nodeId = 3;
00232                 proximitySensorVariables.id = 3;
00233                 proximitySensors.vm.variables = reinterpret_cast<sint16 *>(&proximitySensorVariables);
00234                 proximitySensors.vm.variablesSize = sizeof(proximitySensorVariables) / sizeof(sint16);
00235                 modules.push_back(&proximitySensors);
00236                 
00237                 distanceSensors.vm.nodeId = 4;
00238                 distanceSensorVariables.id = 4;
00239                 distanceSensors.vm.variables = reinterpret_cast<sint16 *>(&distanceSensorVariables);
00240                 distanceSensors.vm.variablesSize = sizeof(distanceSensorVariables) / sizeof(sint16);
00241                 modules.push_back(&distanceSensors);
00242                 
00243                 // fill map
00244                 asebaSocketMaps[&leftMotor.vm] = this;
00245                 asebaSocketMaps[&rightMotor.vm] = this;
00246                 asebaSocketMaps[&proximitySensors.vm] = this;
00247                 asebaSocketMaps[&distanceSensors.vm] = this;
00248                 
00249                 // connect to target
00250                 int port = ASEBA_DEFAULT_PORT + marxbotNumber;
00251                 try
00252                 {
00253                         Dashel::Hub::connect(QString("tcpin:port=%1").arg(port).toStdString());
00254                 }
00255                 catch (Dashel::DashelException e)
00256                 {
00257                         QMessageBox::critical(0, QApplication::tr("Aseba Marxbot"), QApplication::tr("Cannot create listening port %0: %1").arg(port).arg(e.what()));
00258                         abort();
00259                 }
00260                 marxbotNumber++;
00261                 
00262                 // init VM
00263                 AsebaVMInit(&leftMotor.vm);
00264                 AsebaVMInit(&rightMotor.vm);
00265                 AsebaVMInit(&proximitySensors.vm);
00266                 AsebaVMInit(&distanceSensors.vm);
00267         }
00268         
00269         AsebaMarxbot::~AsebaMarxbot()
00270         {
00271                 // clean map
00272                 asebaSocketMaps.erase(&leftMotor.vm);
00273                 asebaSocketMaps.erase(&rightMotor.vm);
00274                 asebaSocketMaps.erase(&proximitySensors.vm);
00275                 asebaSocketMaps.erase(&distanceSensors.vm);
00276         }
00277         
00278         void AsebaMarxbot::controlStep(double dt)
00279         {
00280                 //stepCounter++;
00281                 //std::cerr << stepCounter << std::endl;
00282                 /*
00283                         Values mapping
00284                         
00285                         motor:
00286                                 estimated 3000 == 30 cm/s
00287                         
00288                         encoders:
00289                                 16 tick per motor turn
00290                                 134 reduction
00291                                 6 cm wheel diameter
00292                 */
00293                 
00294                 // set physical variables
00295                 leftSpeed = static_cast<double>(leftMotorVariables.speed) / 100;
00296                 rightSpeed = static_cast<double>(rightMotorVariables.speed) / 100;
00297                 
00298                 // do motion
00299                 DifferentialWheeled::controlStep(dt);
00300                 
00301                 // get physical variables
00302                 int odoLeft = static_cast<int>((leftOdometry * 16  * 134) / (2 * M_PI));
00303                 leftMotorVariables.odo[0] = odoLeft & 0xffff;
00304                 leftMotorVariables.odo[1] = odoLeft >> 16;
00305                 
00306                 int odoRight = static_cast<int>((rightOdometry * 16  * 134) / (2 * M_PI));
00307                 rightMotorVariables.odo[0] = odoRight & 0xffff;
00308                 rightMotorVariables.odo[1] = odoRight >> 16;
00309                 
00310                 for (size_t i = 0; i < 24; i++)
00311                         proximitySensorVariables.bumpers[i] = static_cast<sint16>(getVirtualBumper(i));
00312                 std::fill(proximitySensorVariables.ground, proximitySensorVariables.ground + 12, 0);
00313                 
00314                 for (size_t i = 0; i < 180; i++)
00315                 {
00316                         if (rotatingDistanceSensor.zbuffer[i] > 32767)
00317                                 distanceSensorVariables.distances[i] = 32767;
00318                         else
00319                                 distanceSensorVariables.distances[i] = static_cast<sint16>(rotatingDistanceSensor.zbuffer[i]);
00320                 }
00321                 
00322                 // do a network step
00323                 Hub::step();
00324                 
00325                 for (size_t i = 0; i < modules.size(); i++)
00326                 {
00327                         AsebaVMState* vm = &(modules[i]->vm);
00328                         
00329                         /* no queue for now
00330                         // process all incoming events as long as 
00331                         while (!AsebaVMIsExecutingThread(vm) && !events.empty())
00332                                 AsebaProcessIncomingEvents(&(modules[i]->vm));
00333                         */
00334                         
00335                         // run VM
00336                         AsebaVMRun(vm, 65535);
00337                         
00338                         // reschedule a periodic event if we are not in step by step
00339                         if (AsebaMaskIsClear(vm->flags, ASEBA_VM_STEP_BY_STEP_MASK) || AsebaMaskIsClear(vm->flags, ASEBA_VM_EVENT_ACTIVE_MASK))
00340                                 AsebaVMSetupEvent(vm, ASEBA_EVENT_LOCAL_EVENTS_START);
00341                 }
00342         }
00343         
00344         void AsebaMarxbot::connectionCreated(Dashel::Stream *stream)
00345         {
00346                 std::string targetName = stream->getTargetName();
00347                 if (targetName.substr(0, targetName.find_first_of(':')) == "tcp")
00348                 {
00349                         qDebug() << this << " : New client connected.";
00350                         if (this->stream)
00351                         {
00352                                 closeStream(this->stream);
00353                                 qDebug() << this << " : Disconnected old client.";
00354                         }
00355                         this->stream = stream;
00356                 }
00357         }
00358         
00359         void AsebaMarxbot::incomingData(Stream *stream)
00360         {
00361                 Event event(stream);
00362                 
00363                 // push to other nodes
00364                 for (size_t i = 0; i < modules.size(); ++i)
00365                 {
00366                         Module& module = *(modules[i]);
00367                         module.events.push_back(event);
00368                         AsebaProcessIncomingEvents(&(module.vm));
00369                 }
00370         }
00371         
00372         void AsebaMarxbot::connectionClosed(Dashel::Stream *stream, bool abnormal)
00373         {
00374                 if (stream == this->stream)
00375                 {
00376                         this->stream = 0;
00377                         // clear breakpoints
00378                         for (size_t i = 0; i < modules.size(); i++)
00379                                 (modules[i]->vm).breakpointsCount = 0;
00380                 }
00381                 if (abnormal)
00382                         qDebug() << this << " : Client has disconnected unexpectedly.";
00383                 else
00384                         qDebug() << this << " : Client has disconnected properly.";
00385         }
00386 }
00387 


aseba
Author(s): Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:17:16