Skip to content

Hideaki Tai / omni_main

Go to a project
Commit 907a3c2f by Hideaki Tai
Options

update

parent f0a4bc21
Inline Side-by-side
Showing with 454 additions and 134 deletions
include/global.h
...@@ -4,15 +4,14 @@ ...@@ -4,15 +4,14 @@
#include <Arduino.h> #include <Arduino.h>
#define WHILL_SERIAL_L Serial1 #define WHILL_SERIAL_L Serial2
#define WHILL_SERIAL_R Serial3 #define WHILL_SERIAL_R Serial3
#define XBEE_SERIAL Serial2 #define XBEE_SERIAL Serial1
static constexpr uint8_t MY_ID = 0; static constexpr uint8_t MY_ID = 0;
static constexpr uint32_t WHILL_SERIAL_BAUD = 38400; static constexpr uint32_t WHILL_SERIAL_BAUD = 38400;
static constexpr uint32_t XBEE_SERIAL_BAUD = 38400; static constexpr uint32_t XBEE_SERIAL_BAUD = 38400;
static constexpr uint16_t WHILL_INFO_INTERVAL_MS = 1000; static constexpr uint16_t WHILL_INFO_INTERVAL_MS = 1000;
static constexpr double XBEE_COMMAND_TIMEOUT_MS = 1000.;
#endif // WHILL_GLOBAL_H #endif // WHILL_GLOBAL_H
include/main.h
...@@ -5,12 +5,14 @@ ...@@ -5,12 +5,14 @@
#include <Arduino.h> #include <Arduino.h>
#include <WhillOmni.h> #include <WhillOmni.h>
#include <BorderXBeePacketizer.h> #include <BorderXBeePacketizer.h>
#include <StopWatch.h>
using namespace Whill; using namespace Whill;
WhillOmni whill; WhillOmni whill;
BorderXBeePacker packer; BorderXBeePacker packer;
BorderXBeeUnpacker unpacker; BorderXBeeUnpacker unpacker;
StopWatch stopwatch;
void printSpeedProfile(WhillOmni::LR lr) void printSpeedProfile(WhillOmni::LR lr)
{ {
...@@ -30,7 +32,7 @@ void printSpeedProfile(WhillOmni::LR lr) ...@@ -30,7 +32,7 @@ void printSpeedProfile(WhillOmni::LR lr)
void printSensorInfo(WhillOmni::LR lr) void printSensorInfo(WhillOmni::LR lr)
{ {
Serial.print("SENSOR_INFO"); Serial.println("SENSOR_INFO");
Serial.print("sensor acc x : "); Serial.println(whill.sensorAccelX(lr)); Serial.print("sensor acc x : "); Serial.println(whill.sensorAccelX(lr));
Serial.print("sensor acc y : "); Serial.println(whill.sensorAccelY(lr)); Serial.print("sensor acc y : "); Serial.println(whill.sensorAccelY(lr));
Serial.print("sensor acc z : "); Serial.println(whill.sensorAccelZ(lr)); Serial.print("sensor acc z : "); Serial.println(whill.sensorAccelZ(lr));
...@@ -52,14 +54,43 @@ void printSensorInfo(WhillOmni::LR lr) ...@@ -52,14 +54,43 @@ void printSensorInfo(WhillOmni::LR lr)
void setVelocity() void setVelocity()
{ {
#if 0
unpacker.parse(); unpacker.parse();
while (unpacker.available()) while (unpacker.available())
{ {
if (unpacker.id() == MY_ID) if (unpacker.id() == MY_ID)
{
whill.move(unpacker.velocityX(), unpacker.velocityY(), unpacker.velocityW()); whill.move(unpacker.velocityX(), unpacker.velocityY(), unpacker.velocityW());
}
unpacker.pop(); unpacker.pop();
} }
#else
static float vel[3];
unpacker.parse();
while (unpacker.available())
{
if (unpacker.id() == MY_ID)
{
vel[0] = unpacker.velocityX();
vel[1] = unpacker.velocityY();
vel[2] = unpacker.velocityW();
unpacker.pop();
stopwatch.restart();
}
else if (unpacker.id() == 253)
{
uint32_t status_report_duration = unpacker.statusReportDurationMs();
if (status_report_duration == 0xFFFFFFFF)
whill.streaming(true, status_report_duration, DATASET::SENSOR_INFO);
else
whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
}
}
if (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS) whill.move(vel[0], vel[1], vel[2]);
#endif
} }
void sendStatus(WhillOmni::LR lr) void sendStatus(WhillOmni::LR lr)
...@@ -69,10 +100,11 @@ void sendStatus(WhillOmni::LR lr) ...@@ -69,10 +100,11 @@ void sendStatus(WhillOmni::LR lr)
packer.pack(MY_ID, (uint8_t)lr, whill.battery(lr), whill.errorCode(lr)); packer.pack(MY_ID, (uint8_t)lr, whill.battery(lr), whill.errorCode(lr));
XBEE_SERIAL.write(packer.data(), packer.size()); XBEE_SERIAL.write(packer.data(), packer.size());
#ifdef DEBUG_INFO
if (whill.dataset(lr) == 0) printSpeedProfile(lr); if (whill.dataset(lr) == 0) printSpeedProfile(lr);
else if (whill.dataset(lr) == 1) printSensorInfo(lr); else if (whill.dataset(lr) == 1) printSensorInfo(lr);
else Serial.println("invalid dataset number"); else Serial.println("invalid dataset number");
#endif
whill.pop(lr); whill.pop(lr);
} }
} }
...@@ -84,9 +116,11 @@ void waitReply(WhillOmni::LR lr, uint16_t wait_ms = 0) ...@@ -84,9 +116,11 @@ void waitReply(WhillOmni::LR lr, uint16_t wait_ms = 0)
whill.parse(); whill.parse();
while(whill.available(lr)) while(whill.available(lr))
{ {
#ifdef DEBUG_INFO
if (whill.dataset(lr) == 0) printSpeedProfile(lr); if (whill.dataset(lr) == 0) printSpeedProfile(lr);
else if (whill.dataset(lr) == 1) printSensorInfo(lr); else if (whill.dataset(lr) == 1) printSensorInfo(lr);
else Serial.println("invalid dataset number"); else Serial.println("invalid dataset number");
#endif
whill.pop(lr); whill.pop(lr);
} }
} }
......
lib/BorderXBeePacketizer/BorderXBeePacketizer.h
...@@ -21,7 +21,7 @@ class BorderXBeeUnpacker ...@@ -21,7 +21,7 @@ class BorderXBeeUnpacker
std::array<uint8_t, 8> data; std::array<uint8_t, 8> data;
uint8_t sum; uint8_t sum;
std::array<uint16_t, 3> vel; std::array<float, 3> vel;
void clear() { id = 0xFF; size = 0; data.fill(0); sum = 0; vel.fill(0); } void clear() { id = 0xFF; size = 0; data.fill(0); sum = 0; vel.fill(0); }
}; };
...@@ -29,16 +29,26 @@ class BorderXBeeUnpacker ...@@ -29,16 +29,26 @@ class BorderXBeeUnpacker
public: public:
void attatch(Stream& stream) { serial = &stream; } void attach(Stream& stream) { serial = &stream; }
uint8_t available() { return packets.size(); } uint8_t available() { return packets.size(); }
uint8_t size() { return packets.front().size; } uint8_t size() { return packets.front().size; }
uint8_t data(uint8_t i) { return packets.front().data[i]; } uint8_t data(uint8_t i) { return packets.front().data[i]; }
uint32_t errorcount() { return error_count; }
uint8_t id() { return packets.front().id; } uint8_t id() { return packets.front().id; }
uint16_t velocityX() { return packets.front().vel[0]; } float velocityX() { return packets.front().vel[0]; }
uint16_t velocityY() { return packets.front().vel[1]; } float velocityY() { return packets.front().vel[1]; }
uint16_t velocityW() { return packets.front().vel[2]; } float velocityW() { return packets.front().vel[2]; }
uint32_t statusReportDurationMs()
{
if (status_report_duration == 1) return 50;
else if (status_report_duration == 2) return 100;
else if (status_report_duration == 3) return 500;
else if (status_report_duration == 4) return 1000;
return 0xFFFFFFFF;
}
void pop() { packets.pop(); } void pop() { packets.pop(); }
...@@ -66,24 +76,88 @@ public: ...@@ -66,24 +76,88 @@ public:
} }
case WhillXBeeState::DATA: case WhillXBeeState::DATA:
{ {
#if 0
if (data == BorderXBeeProtocol::BYTE_FOOTER) if (data == BorderXBeeProtocol::BYTE_FOOTER)
{ {
if (buffer.size != 6) if (buffer.size != 7)
{ {
Serial.print("recv data size error!!!! size = "); Serial.print("recv data size error!!!! size = ");
Serial.println(buffer.size); Serial.println(buffer.size);
} }
for (uint8_t i = 0; i < 6; ++i) buffer.sum += buffer.data[i];
if (buffer.sum == buffer.data[buffer.size - 1]) if (buffer.sum == buffer.data[buffer.size - 1])
{
calcurateVel();
packets.push(buffer); packets.push(buffer);
}
else else
{
++error_count; ++error_count;
}
clearPacket(); clearPacket();
break; break;
} }
buffer.data[buffer.size] = data; buffer.data[buffer.size] = data;
++buffer.size; ++buffer.size;
buffer.sum += data; #else
if (buffer.id < 6)
{
if (data == BorderXBeeProtocol::BYTE_FOOTER)
{
if (buffer.size != 7)
{
Serial.print("VEL CMD recv data size error!!!! size = ");
Serial.println(buffer.size);
++error_count;
}
else
{
for (uint8_t i = 0; i < 6; ++i) buffer.sum += buffer.data[i];
if (buffer.sum == buffer.data[buffer.size - 1])
{
calcurateVel();
packets.push(buffer);
}
else
{
++error_count;
}
}
clearPacket();
break;
}
buffer.data[buffer.size] = data;
++buffer.size;
}
else if (buffer.id == 253) // change status report mode
{
if (data == BorderXBeeProtocol::BYTE_FOOTER)
{
if (buffer.size != 1)
{
Serial.print("STATUS RPT recv data size error!!!! size = ");
Serial.println(buffer.size);
++error_count;
}
else
{
status_report_duration = buffer.data[0];
}
clearPacket();
break;
}
buffer.data[buffer.size] = data;
++buffer.size;
}
else
{
Serial.print("undefined CMD error!!!! CMD ID = ");
Serial.println(buffer.id);
++error_count;
clearPacket();
}
#endif
break; break;
} }
default: default:
...@@ -102,11 +176,14 @@ private: ...@@ -102,11 +176,14 @@ private:
{ {
float spd = (float)(((uint16_t)buffer.data[1] << 8) | (uint16_t)buffer.data[0]); // [mm/s] float spd = (float)(((uint16_t)buffer.data[1] << 8) | (uint16_t)buffer.data[0]); // [mm/s]
float rad = (float)(((uint16_t)buffer.data[3] << 8) | (uint16_t)buffer.data[2]) / 10000.f; // [rad] float rad = (float)(((uint16_t)buffer.data[3] << 8) | (uint16_t)buffer.data[2]) / 10000.f; // [rad]
float rot = (float)(((uint16_t)buffer.data[5] << 8) | (uint16_t)buffer.data[4]) - 1000.f; // [rad/s] ?? float rot = ((float)(((uint16_t)buffer.data[5] << 8) | (uint16_t)buffer.data[4]) - 1000.f) / 100.f; // [rad/s]
// convert coordinate
rad += HALF_PI;
buffer.vel[0] = spd * cos(rad) / 1000.f; // [m/s] buffer.vel[0] = spd * cos(rad); // [mm/s]
buffer.vel[1] = spd * sin(rad) / 1000.f; // [m/s] buffer.vel[1] = spd * sin(rad); // [mm/s]
buffer.vel[2] = map(rot, -1000.f, 1000.f, -PI, PI); // [rad/s] ?? buffer.vel[2] = rot; // [rad/s]
} }
void clearPacket() { buffer.clear(); state = WhillXBeeState::Header; } void clearPacket() { buffer.clear(); state = WhillXBeeState::Header; }
...@@ -116,6 +193,7 @@ private: ...@@ -116,6 +193,7 @@ private:
std::queue<WhillXBeeData> packets; std::queue<WhillXBeeData> packets;
WhillXBeeState state; WhillXBeeState state;
uint32_t error_count; uint32_t error_count;
uint8_t status_report_duration;
}; };
......
lib/StopWatch/StopWatch.h 0 → 100644
#pragma once
#ifndef STOPWATCH_H
#define STOPWATCH_H
class StopWatch
{
public:
void start() { play(); us_start_ = micros(); }
void stop() { pause(); us_start_ = 0; }
void restart() { stop(); start(); }
void play() { is_running_ = true; us_elapsed_ = us(); }
void pause() { is_running_ = false; us_start_ = micros() - us_elapsed_; }
bool isRunning() { return is_running_; }
inline void setCurrTime(double s)
{
us_offset_ += s * 1000000. - us();
}
void setOffsetUs(double us_offset) { us_offset_ = us_offset; }
// TODO: how to handle over int32_t range
inline double us() const
{
if (us_start_ == 0) return 0;
return (double)micros() - us_start_ + us_offset_;
}
inline double ms() const { return us() * 0.001; }
inline double sec() const { return us() * 0.000001; }
private:
double us_start_ {0.0};
double us_elapsed_ {0.0};
double us_offset_ {0.0};
bool is_running_ {false};
};
class FrameRateCounter : public StopWatch
{
public:
FrameRateCounter(double fps = 40.0)
: fps_(fps)
, interval_(getTargetIntervalMicros())
, curr_frame_(0)
, prev_frame_(0)
, b_zero_start_(false)
{
}
double frame()
{
return us() / interval_;
}
bool isNextFrame()
{
bool b;
double curr_frame = frame();
if (b_zero_start_) b = (floor(curr_frame) != floor(prev_frame_));
else b = (ceil(curr_frame) != ceil(prev_frame_));
prev_frame_ = curr_frame;
return b;
}
void setFrameRate(double fps)
{
fps_ = fps;
interval_ = getTargetIntervalMicros();
}
void setZeroStart(bool b) { b_zero_start_ = b; }
private:
double getTargetIntervalMicros() const { return (1000000.0 / fps_); }
double fps_ {40.0};
double interval_ {0.0};
double curr_frame_ {0};
double prev_frame_ {0};
bool b_zero_start_ {true};
};
#endif // STOPWATCH_H
lib/Whill/OmniModel.h
...@@ -21,8 +21,9 @@ namespace Work ...@@ -21,8 +21,9 @@ namespace Work
const double g = 9.80665; // gravitational acceleraation [m/s^2] const double g = 9.80665; // gravitational acceleraation [m/s^2]
const double R = 0.1; // wheel radius [m] const double R = 0.12; // wheel radius [m]
const double W = 0.2; // wheel base / 2.0 [m] (from center to wheel) // const double W = 0.2; // wheel base / 2.0 [m] (from center to wheel)
const double L = 0.43; // from center to wheel [m]
// const double T = 0.29; // tread / 2.0 [m] (from center to wheel) // const double T = 0.29; // tread / 2.0 [m] (from center to wheel)
const double Mp = 10.0; // mass of platform [kg] const double Mp = 10.0; // mass of platform [kg]
const double Ml = 10.0; // mass of load [kg] const double Ml = 10.0; // mass of load [kg]
...@@ -33,9 +34,9 @@ namespace Work ...@@ -33,9 +34,9 @@ namespace Work
// const double L = T + W; // const double L = T + W;
// const double A = (Mp + Ml) * R * R / 8.0; // const double A = (Mp + Ml) * R * R / 8.0;
// const double B = Ip * R * R / (16.0 * L * L); // const double B = Ip * R * R / (16.0 * L * L);
const double AA = Mp * R * R / 2.0; // const double AA = Mp * R * R / 2.0;
const double BB = Ip * R * R / (4.0 * W * W); // const double BB = Ip * R * R / (4.0 * W * W);
const double CC = (AA + BB) / (R * R); // const double CC = (AA + BB) / (R * R);
const double wheel_thickness = 0.05; const double wheel_thickness = 0.05;
...@@ -57,38 +58,39 @@ namespace Work ...@@ -57,38 +58,39 @@ namespace Work
// C_inv = PseudoInverse(C); // C_inv = PseudoInverse(C);
I << // I <<
AA + BB, -AA, BB, // AA + BB, -AA, BB,
-AA, 2. * AA, -AA, // -AA, 2. * AA, -AA,
BB, -AA, CC; // BB, -AA, CC;
I_inv = I.inverse(); // I_inv = I.inverse();
this->Jacobian_local << this->Jacobian_inv <<
-1., 1., sqrt(2.) * W, -sin(PI * 1. / 4.), cos(PI * 1. / 4.), L,
1., 1., sqrt(2.) * W, -sin(PI * 3. / 4.), cos(PI * 3. / 4.), L,
1., -1., sqrt(2.) * W, -sin(PI * 5. / 4.), cos(PI * 5. / 4.), L,
-1., -1., sqrt(2.) * W; -sin(PI * 7. / 4.), cos(PI * 7. / 4.), L;
this->Jacobian_inv /= R;
this->Jacobian_local = PseudoInverse(this->Jacobian_inv);
this->Jacobian_local /= sqrt(2.) * R; // updateJacobian(0.0);
updateJacobian(0.0);
this->Jacobian_tra = this->Jacobian.transpose(); // this->Jacobian_tra = this->Jacobian.transpose();
Matrix<double, DmsJ, DmsJ> J_null_tmp = this->Jacobian_inv * this->Jacobian; // Matrix<double, DmsJ, DmsJ> J_null_tmp = this->Jacobian_inv * this->Jacobian;
this->Jacobian_null = MatrixXd(DmsJ, DmsJ).setIdentity() - J_null_tmp; // this->Jacobian_null = MatrixXd(DmsJ, DmsJ).setIdentity() - J_null_tmp;
MatrixXd M_buff(DmsW, DmsJ); // MatrixXd M_buff(DmsW, DmsJ);
M_buff = this->Jacobian * I_inv; // M_buff = this->Jacobian * I_inv;
this->M_inv = M_buff * this->Jacobian_tra; // this->M_inv = M_buff * this->Jacobian_tra;
this->Mn.setIdentity(); // this->Mn.setIdentity();
this->Mn_inv = this->Mn.inverse(); // this->Mn_inv = this->Mn.inverse();
for (size_t i = 0; i < DmsJ; ++i) In(i, i) = I(i, i); // for (size_t i = 0; i < DmsJ; ++i) In(i, i) = I(i, i);
} }
...@@ -154,9 +156,10 @@ namespace Work ...@@ -154,9 +156,10 @@ namespace Work
// } // }
const double& getWheelBase() const { return W; } // const double& getWheelBase() const { return W; }
// const double& getTread() const { return T; } // const double& getTread() const { return T; }
const double& getDiameter() const { return R; } double getWheelRadius() const { return R / 2.0; }
const double& getWheelDiameter() const { return R; }
const double& getWheelThickness() const { return wheel_thickness; } const double& getWheelThickness() const { return wheel_thickness; }
}; };
......
lib/Whill/Whill.cpp
...@@ -11,7 +11,7 @@ bool WhillModelC::open(Stream& device, int baud) ...@@ -11,7 +11,7 @@ bool WhillModelC::open(Stream& device, int baud)
return true; return true;
} }
void WhillModelC::attatch(Stream& device) void WhillModelC::attach(Stream& device)
{ {
serial = &device; serial = &device;
} }
...@@ -39,35 +39,36 @@ bool WhillModelC::controlEnable(bool b) ...@@ -39,35 +39,36 @@ bool WhillModelC::controlEnable(bool b)
return this->send(); return this->send();
} }
bool WhillModelC::move(char fb, char lr) bool WhillModelC::move(int8_t fb, int8_t lr) // fb: 100 f, -100 b, lr -100 = L, 100 = R
{ {
Serial.print("Whill move : ");
Serial.print((int)fb);
Serial.print(" ");
Serial.println((int)lr);
packer.joystick(fb, lr); packer.joystick(fb, lr);
return this->send(); return this->send();
} }
bool WhillModelC::moveVel(float x, float y) // [m/s] bool WhillModelC::moveVel(float y, float x) // [m/s]
{ {
int8_t fb, lr; float fb, lr;
fb = (int8_t)(y / spd_profile.f_m1 * 100.f); fb = y / spd_profile.f_m1 * 100.f;
lr = (int8_t)(y / spd_profile.t_m1 * 100.f); lr = x / spd_profile.t_m1 * 100.f;
if (abs(fb) > 100) if (abs(fb) > 100)
{ {
Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(fb); Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(fb);
fb = (int8_t)constrain(fb, -100, 100); fb = constrain(fb, -100, 100);
} }
if (abs(lr) > 100) if (abs(lr) > 100)
{ {
Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(lr); Serial.print("Warning! Excess WHILL speed Limit : "); Serial.println(lr);
lr = (int8_t)constrain(lr, -100, 100); lr = constrain(lr, -100, 100);
} }
return this->move(fb, lr); // Serial.println("move 0-100 (y, x)");
// Serial.print(fb);
// Serial.print(", ");
// Serial.println(lr);
return this->move((int8_t)fb, (int8_t)lr);
} }
bool WhillModelC::stop() bool WhillModelC::stop()
...@@ -113,14 +114,6 @@ void WhillModelC::parse() ...@@ -113,14 +114,6 @@ void WhillModelC::parse()
bool WhillModelC::send() bool WhillModelC::send()
{ {
Serial.print("data : ");
for (uint8_t i = 0; i < packer.size(); ++i)
{
Serial.print((int)packer.data()[i]);
Serial.print(" ");
}
Serial.println();
int n = serial->write(packer.data(), packer.size()); int n = serial->write(packer.data(), packer.size());
packer.clear(); // TODO: smarter way... packer.clear(); // TODO: smarter way...
return (n == packer.size()); return (n == packer.size());
......
lib/Whill/Whill.h
...@@ -12,23 +12,23 @@ class WhillModelC ...@@ -12,23 +12,23 @@ class WhillModelC
{ {
struct SpeedProfile struct SpeedProfile
{ {
float f_m1; float f_m1; // [m/s]
float r_m1; float r_m1; // [m/s]
float t_m1; float t_m1; // [m/s]?? TODO: check spec
}; };
public: public:
bool open(Stream& device, int baud); bool open(Stream& device, int baud);
void attatch(Stream& device); void attach(Stream& device);
void close(); void close();
bool power(bool b); bool power(bool b);
bool powerSupply(bool b); bool powerSupply(bool b);
bool controlEnable(bool b); bool controlEnable(bool b);
bool move(char fb, char lr); bool move(int8_t fb, int8_t lr);
bool moveVel(float x, float y); bool moveVel(float y, float x);
bool stop(); bool stop();
bool speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1); bool speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1);
...@@ -75,6 +75,14 @@ public: ...@@ -75,6 +75,14 @@ public:
uint8_t speedModeIndicator() { return unpacker.speedModeIndicator(); } uint8_t speedModeIndicator() { return unpacker.speedModeIndicator(); }
uint8_t errorCode() { return unpacker.errorCode(); } uint8_t errorCode() { return unpacker.errorCode(); }
// 34 バッテリー通信エラー
// 61 モーター高温
// 62 モーター低温
// 70 モータードライバ温度異常
// 71 モーター電圧がover voltage
// 72 モーター電圧がunder voltage
// 74 モーター過電流
private: private:
bool send(); bool send();
......
lib/Whill/WhillOmni.cpp
...@@ -13,10 +13,10 @@ bool WhillOmni::open(Stream& device_l, Stream& device_r, int baud) ...@@ -13,10 +13,10 @@ bool WhillOmni::open(Stream& device_l, Stream& device_r, int baud)
return true; return true;
} }
void WhillOmni::attatch(Stream& device_l, Stream& device_r) void WhillOmni::attach(Stream& device_l, Stream& device_r)
{ {
whills[(uint8_t)LR::L].attatch(device_l); whills[(uint8_t)LR::L].attach(device_l);
whills[(uint8_t)LR::R].attatch(device_r); whills[(uint8_t)LR::R].attach(device_r);
} }
void WhillOmni::close() void WhillOmni::close()
...@@ -45,68 +45,52 @@ bool WhillOmni::controlEnable(bool b) ...@@ -45,68 +45,52 @@ bool WhillOmni::controlEnable(bool b)
return r; return r;
} }
bool WhillOmni::move(float x, float y, float r) bool WhillOmni::move(float x, float y, float r) // [mm/s], [mm/s], [rad/s]
{ {
#if 0 model.updateJacobian(0.0);
float vl[2], vr[2]; // virtual velocity of whill motors
WhillXY wxy[2]; // virtual WHILL's XY velocity
// convert joystick x y r -> motors // 1: L = 1R, 2: T = 0L, 3: R = 0R, 4: B = 1L
vr[1] = x * C + y * C + RADIUS * r; VectorXd v_omni(3);
vl[0] = -x * C + y * C + RADIUS * r; VectorXd w_wheel(4);
vr[0] = -x * C - y * C + RADIUS * r; VectorXd whill_l(2);
vl[1] = x * C - y * C + RADIUS * r; VectorXd whill_r(2);
// TODO: force conversion (direction)
// v_omni << x / 1000.f, -y / 1000.f, -r; // [m/s], [m/s], [rad/s]
v_omni << x / 1000.f, y / 1000.f, r; // [m/s], [m/s], [rad/s]
w_wheel = model.Jacobian_inv * v_omni;
// convert motors -> WHILL's XY // convert motors -> WHILL's XY
wxy[0].x = vr[0] + vl[0]; // 0: whill[0] L, 1: whill[0] R, 2: whill[1] L, 3: whill[2] R
wxy[0].y = (vl[0] - vr[0]) / 2.f;
// wxy[0].x *= 25.f; // TODO: ??
// wxy[0].y *= 25.f; // TODO: ??
wxy[1].x = vr[1] + vl[1];
wxy[1].y = (vl[1] - vr[1]) / 2.f;
// wxy[1].x *= 25.f; // TODO: ??
// wxy[1].y *= 25.f; // TODO: ??
Serial.println("move :");
bool ret = true;
for (int i = 0; i < NUM_WHILLS; ++i)
{
Serial.print((int)wxy[i].y); Serial.print(" "); Serial.println((int)wxy[i].x);
ret &= whills[i].move((int8_t)wxy[i].y, (int8_t)wxy[i].x);
}
return ret;
#else // Serial.println("wheel speed");
// Serial.print(w_wheel[0]); Serial.print(", ");
// Serial.print(w_wheel[1]); Serial.print(", ");
// Serial.print(w_wheel[2]); Serial.print(", ");
// Serial.print(w_wheel[3]); Serial.println();
model.updateJacobian(0.0); // convert motor direction to whill
w_wheel[0] = -w_wheel[0];
w_wheel[2] = -w_wheel[2];
// 1: L = 1R, 2: T = 0L, 3: R = 0R, 4: B = 1L // calcurate whill's translation speed
VectorXd v_omni; whill_l.y() = (w_wheel[1] + w_wheel[0]) / 2.f * model.getWheelRadius();
VectorXd w_wheel; whill_l.x() = (w_wheel[1] - w_wheel[0]) / 0.55f * model.getWheelRadius();
VectorXd whill[2]; whill_r.y() = (w_wheel[3] + w_wheel[2]) / 2.f * model.getWheelRadius();
whill_r.x() = (w_wheel[3] - w_wheel[2]) / 0.55f * model.getWheelRadius();
v_omni << x, y, r; // [m/s], [m/s], [rad/s](??) // convert whill direction
w_wheel = model.Jacobian_inv * v_omni; whill_l.x() = -whill_l.x();
whill_r.x() = -whill_r.x();
// convert motors -> WHILL's XY // Serial.println("move [m/s] (y, x) :");
whill[0].x() = w_wheel[1] + w_wheel[2]; // Serial.print(whill_l.y()); Serial.print(" "); Serial.println(whill_l.x());
whill[0].y() = (w_wheel[1] - w_wheel[2]) / 2.f; // Serial.print(whill_r.y()); Serial.print(" "); Serial.println(whill_r.x());
whill[1].x() = w_wheel[3] + w_wheel[0];
whill[1].y() = (w_wheel[3] - w_wheel[0]) / 2.f;
Serial.println("move :");
bool ret = true; bool ret = true;
for (int i = 0; i < NUM_WHILLS; ++i) ret &= whills[0].moveVel(whill_l.y(), whill_l.x());
{ ret &= whills[1].moveVel(whill_r.y(), whill_r.x());
Serial.print((int)whill[i].y()); Serial.print(" "); Serial.println((int)whill[i].x());
ret &= whills[i].moveVel(whill[i].y(), whill[i].x());
}
return ret; return ret;
#endif
} }
bool WhillOmni::stop() bool WhillOmni::stop()
......
lib/Whill/WhillOmni.h
...@@ -17,14 +17,14 @@ public: ...@@ -17,14 +17,14 @@ public:
enum class LR { L, R }; enum class LR { L, R };
bool open(Stream& device_l, Stream& device_r, int baud); bool open(Stream& device_l, Stream& device_r, int baud);
void attatch(Stream& device_l, Stream& device_r); void attach(Stream& device_l, Stream& device_r);
void close(); void close();
bool power(bool b); bool power(bool b);
bool powerSupply(bool b); bool powerSupply(bool b);
bool controlEnable(bool b); bool controlEnable(bool b);
bool move(float x, float y, float r); // [mm/s] bool move(float x, float y, float r); // [mm/s], [mm/s], [rad/s]
bool stop(); bool stop();
bool speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1); bool speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_m1, char r_a1, char r_d1, char t_m1, char t_a1, char t_d1);
......
lib/Whill/test/01_datastreaming/01_datastreaming.ino
...@@ -72,7 +72,7 @@ void parseReply() ...@@ -72,7 +72,7 @@ void parseReply()
void setup() void setup()
{ {
Serial1.begin(38400, SERIAL_8N2); Serial1.begin(38400, SERIAL_8N2);
whill.attatch(Serial1); whill.attach(Serial1);
delay(1000); delay(1000);
......
lib/XBeeATCmds/XBeeATCmds.h 0 → 100644
class XBeeATCmds
{
public:
void attach(Stream& s)
{
serial = &s;
}
bool applyChange()
{
Serial.printf("APPLY CHANGE: \n");
serial->printf("ATAC\r");
return dumpReply();
}
bool reset()
{
Serial.printf("RESET: \n");
serial->printf("ATFR\r");
return dumpReply();
}
bool writeToMemory()
{
Serial.printf("WRITE TO MEMORY: \n");
serial->printf("ATWR\r");
return dumpReply();
}
bool setChannelMask(uint64_t mask)
{
Serial.printf("SET CHANNEL MASK: \n");
char mask_str[17] = {'\0'};
uint64_to_hex(mask, mask_str);
Serial.printf("%s\n", mask_str);
serial->printf("ATCM %s\r", mask_str);
return dumpReply();
}
bool setPreambleID(uint8_t id)
{
Serial.printf("SET PREAMBLE ID: \n");
char id_str[3] = {'\0'};
uint8_to_hex(id, id_str);
Serial.printf("%s\n", id_str);
serial->printf("ATHP %s\r", id_str);
return dumpReply();
}
bool setNetworkID(uint16_t id)
{
Serial.printf("SET NETWORK ID: \n");
char id_str[5] = {'\0'};
uint16_to_hex(id, id_str);
Serial.println(id_str);
serial->printf("ATID %s\r", id_str);
return dumpReply();
}
bool setBroadcastMultiTransmit(uint8_t n_times)
{
Serial.printf("SET BROADCAST MULTITRANSMIT: \n");
char n_times_str[3] = {'\0'};
uint8_to_hex(n_times, n_times_str);
Serial.println(n_times_str);
serial->printf("ATMT %s\r", n_times_str);
return dumpReply();
}
bool setDestinationAddr(uint32_t addr_h, uint32_t addr_l)
{
Serial.printf("SET DESTINATION ADDR: \n");
char addr_h_str[9] = {'\0'};
char addr_l_str[9] = {'\0'};
uint32_to_hex(addr_h, addr_h_str);
uint32_to_hex(addr_l, addr_l_str);
Serial.println(addr_h_str);
serial->printf("ATDH %s\r", addr_h_str);
bool b = dumpReply();
Serial.println(addr_l_str);
serial->printf("ATDL %s\r", addr_l_str);
return (b & dumpReply());
}
bool setBaudRate(uint8_t baudrate)
{
Serial.printf("SET BAUDRATE: \n");
char baudrate_str[3] = {'\0'};
uint8_to_hex(baudrate, baudrate_str);
Serial.println(baudrate_str);
serial->printf("ATBD %s\r", baudrate_str);
return dumpReply();
}
bool enterCommandMode()
{
Serial.printf("ENTER COMMAND MODE: \n");
serial->printf("+++");
delay(1000);
return dumpReply();
}
bool exitCommandMode()
{
Serial.printf("EXIT COMMAND MODE: \n");
serial->printf("ATCN\r");
return dumpReply();
}
bool dumpReply()
{
delay(XB_REPLY_WAIT);
if (!serial->available()) {
Serial.println("XBee NOT Responding... Timeout...");
return false;
}
while(serial->available()) Serial.write(XB_SERIAL.read());
Serial.println();
return true;
}
private:
Stream* serial;
};
src/main.cpp
// #define DEBUG_INFO
#include "global.h" #include "global.h"
#include "main.h" #include "main.h"
void setup() void setup()
{ {
Serial.begin(115200);
delay(2000);
WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L
WHILL_SERIAL_R.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // R WHILL_SERIAL_R.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // R
whill.attatch(WHILL_SERIAL_L, WHILL_SERIAL_R); whill.attach(WHILL_SERIAL_L, WHILL_SERIAL_R);
XBEE_SERIAL.begin(XBEE_SERIAL_BAUD); // XBee XBEE_SERIAL.begin(XBEE_SERIAL_BAUD); // XBee
unpacker.attach(XBEE_SERIAL);
delay(2000); whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SPEED_PROFILE);
whill.stop(); whill.stop();
delay(2000); delay(2000);
// WHILL setting
Serial.println("set speed profile");
whill.speed(SPEEDMODE::RS232C, 60, 90, 160, 60, 50, 80, 60, 60, 160); // max whill.speed(SPEEDMODE::RS232C, 60, 90, 160, 60, 50, 80, 60, 60, 160); // max
whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO); delay(1000);
waitReply(WhillOmni::LR::L, WHILL_INFO_INTERVAL_MS + 500);
Serial.println("check speed profile");
whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SPEED_PROFILE); // valid in next streaming phase
waitReply(WhillOmni::LR::L, WHILL_INFO_INTERVAL_MS + 1000);
waitReply(WhillOmni::LR::R); waitReply(WhillOmni::LR::R);
Serial.println("change streaming to sensor info");
whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO); whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
delay(1000); delay(1000);
......
  • Markdown is supported
    0% or
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or sign in to comment