Merge branch 'master' of https://github.com/ROBOTIS-GIT/OpenCR.git

arduino/opencr_arduino/opencr/cores/arduino/IPAddress.cpp

@@ -0,0 +1,114 @@
+/*
+  IPAddress.cpp - Base class that provides IPAddress
+  Copyright (c) 2011 Adrian McEwen.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library 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
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <Arduino.h>
+#include <IPAddress.h>
+
+IPAddress::IPAddress()
+{
+    _address.dword = 0;
+}
+
+IPAddress::IPAddress(uint8_t first_octet, uint8_t second_octet, uint8_t third_octet, uint8_t fourth_octet)
+{
+    _address.bytes[0] = first_octet;
+    _address.bytes[1] = second_octet;
+    _address.bytes[2] = third_octet;
+    _address.bytes[3] = fourth_octet;
+}
+
+IPAddress::IPAddress(uint32_t address)
+{
+    _address.dword = address;
+}
+
+IPAddress::IPAddress(const uint8_t *address)
+{
+    memcpy(_address.bytes, address, sizeof(_address.bytes));
+}
+
+bool IPAddress::fromString(const char *address)
+{
+    uint16_t acc = 0; // Accumulator
+    uint8_t dots = 0;
+
+    while (*address)
+    {
+        char c = *address++;
+        if (c >= '0' && c <= '9')
+        {
+            acc = acc * 10 + (c - '0');
+            if (acc > 255) {
+                // Value out of [0..255] range
+                return false;
+            }
+        }
+        else if (c == '.')
+        {
+            if (dots == 3) {
+                // Too much dots (there must be 3 dots)
+                return false;
+            }
+            _address.bytes[dots++] = acc;
+            acc = 0;
+        }
+        else
+        {
+            // Invalid char
+            return false;
+        }
+    }
+
+    if (dots != 3) {
+        // Too few dots (there must be 3 dots)
+        return false;
+    }
+    _address.bytes[3] = acc;
+    return true;
+}
+
+IPAddress& IPAddress::operator=(const uint8_t *address)
+{
+    memcpy(_address.bytes, address, sizeof(_address.bytes));
+    return *this;
+}
+
+IPAddress& IPAddress::operator=(uint32_t address)
+{
+    _address.dword = address;
+    return *this;
+}
+
+bool IPAddress::operator==(const uint8_t* addr) const
+{
+    return memcmp(addr, _address.bytes, sizeof(_address.bytes)) == 0;
+}
+
+size_t IPAddress::printTo(Print& p) const
+{
+    size_t n = 0;
+    for (int i =0; i < 3; i++)
+    {
+        n += p.print(_address.bytes[i], DEC);
+        n += p.print('.');
+    }
+    n += p.print(_address.bytes[3], DEC);
+    return n;
+}
+

arduino/opencr_arduino/opencr/cores/arduino/IPAddress.h

@@ -21,7 +21,8 @@
 #define IPAddress_h
 
 #include <stdint.h>
-#include <Printable.h>
+#include "Printable.h"
+#include "WString.h"
 
 // A class to make it easier to handle and pass around IP addresses
 
@@ -45,6 +46,9 @@ class IPAddress : public Printable {
     IPAddress(uint32_t address);
     IPAddress(const uint8_t *address);
 
+    bool fromString(const char *address);
+    bool fromString(const String &address) { return fromString(address.c_str()); }
+
     // Overloaded cast operator to allow IPAddress objects to be used where a pointer
     // to a four-byte uint8_t array is expected
     operator uint32_t() const { return _address.dword; };
@@ -71,5 +75,4 @@ class IPAddress : public Printable {
 
 const IPAddress INADDR_NONE(0,0,0,0);
 
-
 #endif

arduino/opencr_arduino/opencr/libraries/DynamixelWorkbench/src/dynamixel_workbench_toolbox/dynamixel_driver.cpp

@@ -328,7 +328,7 @@ bool DynamixelDriver::scan(uint8_t *get_id, uint8_t *get_the_number_of_id, uint8
 
 bool DynamixelDriver::scan(uint8_t *get_id, uint8_t *get_the_number_of_id, uint8_t range, const char **log)
 {
-  return scan(get_id, get_the_number_of_id, 1, range, log);
+  return scan(get_id, get_the_number_of_id, 0, range, log);
 }
 
 bool DynamixelDriver::ping(uint8_t id, uint16_t *get_model_number, const char **log)
@@ -570,9 +570,9 @@ bool DynamixelDriver::writeRegister(uint8_t id, uint16_t address, uint16_t lengt
   ErrorFromSDK sdk_error = {0, false, false, 0};
 
 #if defined(__OPENCR__) || defined(__OPENCM904__)
-    delay(50);
+    delay(10);
 #else
-    usleep(1000*50);
+    usleep(1000*10);
 #endif
 
   sdk_error.dxl_comm_result = packetHandler_->writeTxRx(portHandler_, 
@@ -681,9 +681,9 @@ bool DynamixelDriver::writeOnlyRegister(uint8_t id, uint16_t address, uint16_t l
   ErrorFromSDK sdk_error = {0, false, false, 0};
 
 #if defined(__OPENCR__) || defined(__OPENCM904__)
-    delay(50);
+    delay(10);
 #else
-    usleep(1000*50);
+    usleep(1000*10);
 #endif
 
   sdk_error.dxl_comm_result = packetHandler_->writeTxOnly(portHandler_, 
@@ -718,9 +718,9 @@ bool DynamixelDriver::writeOnlyRegister(uint8_t id, const char *item_name, int32
   if (control_item == NULL) return false;
 
 #if defined(__OPENCR__) || defined(__OPENCM904__)
-    delay(50);
+    delay(10);
 #else
-    usleep(1000*50);
+    usleep(1000*10);
 #endif
 
   switch (control_item->data_length)

arduino/opencr_arduino/opencr/libraries/OpenManipulator/example/Chain/open_manipulator_chain_pen/OpenManipulatorPenDrawing.cpp

@@ -30,7 +30,6 @@ void Alphabet::initAlphabet(double move_time, double control_time, std::vector<W
   start_pose_ = start;
   alphabet_ = alphabet;
   move_time_ = move_time;
-  //scale_ = 0.010;
   scale_ = (double)scale * 0.001 *0.5;
 
   WayPoint drawingStart, drawingGoal;
@@ -94,50 +93,6 @@ void Alphabet::init(double move_time, double control_time, std::vector<WayPoint>
 std::vector<WayPoint> Alphabet::getJointWayPoint(double tick){ return {}; }
 std::vector<WayPoint> Alphabet::getTaskWayPoint(double tick){  return drawAlphabet(tick); }
 
-
-std::vector<WayPoint> Alphabet::drawing_A(double t)
-{
-  // set drawing trajectory
-  std::vector<WayPoint> pose;
-  pose.resize(6);
-  double diff_pose[2] = {0.0, 0.0};
-
-  if(t <= 1.0)
-  {
-    double x = t;         
-    diff_pose[0] =  x;// x
-    diff_pose[1] =  2 * (x);// y
-  }
-  else if(t <= 2.0)
-  {
-    double x = t;
-    diff_pose[0] =  x;// x
-    diff_pose[1] =  -2 * (x) + 4;// y
-  }
-  else if(t <= 2.5)
-  {
-    double x = 2.0 - (t - 2.0);
-    diff_pose[0] =  x;// x
-    diff_pose[1] =  -2 * (x) + 4;// y
-  }
-  else
-  {
-    double x = 1.5 - (t - 2.5);
-    diff_pose[0] =  x-0.1;// x
-    diff_pose[1] =  1;// y
-  }
-
-  pose.at(0).value = start_pose_.at(0).value - diff_pose[1]*scale_;
-  pose.at(1).value = start_pose_.at(1).value + diff_pose[0]*scale_;
-  pose.at(2).value = start_pose_.at(2).value;
-
-  pose.at(3).value = start_pose_.at(3).value;
-  pose.at(4).value = start_pose_.at(4).value;
-  pose.at(5).value = start_pose_.at(5).value;
-
-  return pose;
-}
-
 std::vector<WayPoint> Alphabet::drawing_B(double t)
 {
   // set drawing trajectory

arduino/opencr_arduino/opencr/libraries/OpenManipulator/example/Chain/open_manipulator_chain_pen/OpenManipulatorPenDrawing.h

@@ -49,7 +49,6 @@ class Alphabet : public ROBOTIS_MANIPULATOR::DrawingTrajectory
 
   void initAlphabet(double move_time, double control_time, std::vector<WayPoint> start, char alphabet, char scale);
   std::vector<WayPoint> drawAlphabet(double time_var);
-  std::vector<WayPoint> drawing_A(double t);
   std::vector<WayPoint> drawing_B(double t);
   std::vector<WayPoint> drawing_R(double t);
   std::vector<WayPoint> drawing_C(double t);