Skip to content

Hideaki Tai / omni_main

Go to a project
Commit f8d3bf60 by Hideaki Tai
Options

緊急停止、常にWHILLのデータ受信、任意間隔でステータス送信、WHILL 通信監視、1回でLR送信

parent 70724233
Inline Side-by-side
Showing with 202 additions and 89 deletions
include/global.h
...@@ -12,10 +12,17 @@ static constexpr uint32_t WHILL_SERIAL_BAUD = 38400; ...@@ -12,10 +12,17 @@ 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.; static constexpr double XBEE_COMMAND_TIMEOUT_MS = 100.;
uint8_t HARDWARE_ID = 0xFF; uint8_t HARDWARE_ID = 0xFF;
bool b_need_xbee_init = false; bool b_need_xbee_init = false;
static constexpr uint8_t PIN_EMERGENCY = 32;
static constexpr uint8_t PIN_IRLED_OE = 17;
bool b_motor_active = true;
bool b_streaming = true;
uint16_t streaming_interval = 1000;
struct XBeeSettings struct XBeeSettings
{ {
uint8_t channel; uint8_t channel;
...@@ -24,7 +31,6 @@ struct XBeeSettings ...@@ -24,7 +31,6 @@ struct XBeeSettings
uint32_t dst_addr_l; uint32_t dst_addr_l;
uint8_t packetization_timeout; uint8_t packetization_timeout;
}; };
XBeeSettings xb_settings[6]; XBeeSettings xb_settings[6];
#endif // WHILL_GLOBAL_H #endif // WHILL_GLOBAL_H
include/main.h
...@@ -25,7 +25,6 @@ StopWatch stopwatch; ...@@ -25,7 +25,6 @@ StopWatch stopwatch;
void printSpeedProfile(WhillOmni::LR lr) void printSpeedProfile(WhillOmni::LR lr)
{ {
Serial.println("SPEED_PROFILE"); Serial.println("SPEED_PROFILE");
Serial.print("dataset : "); Serial.println(whill.dataset(lr));
Serial.print("mode : "); Serial.println(whill.speedMode(lr)); Serial.print("mode : "); Serial.println(whill.speedMode(lr));
Serial.print("fwd spd : "); Serial.println(whill.forwardSpeed(lr)); Serial.print("fwd spd : "); Serial.println(whill.forwardSpeed(lr));
Serial.print("fwd acc : "); Serial.println(whill.forwardAccel(lr)); Serial.print("fwd acc : "); Serial.println(whill.forwardAccel(lr));
...@@ -239,11 +238,12 @@ void setVelocity() ...@@ -239,11 +238,12 @@ void setVelocity()
if (status_report_duration == 0xFFFFFFFF) if (status_report_duration == 0xFFFFFFFF)
{ {
whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO); b_streaming = false;
} }
else else
{ {
whill.streaming(true, status_report_duration, DATASET::SENSOR_INFO); b_streaming = true;
streaming_interval = status_report_duration;
} }
} }
...@@ -261,23 +261,17 @@ void setVelocity() ...@@ -261,23 +261,17 @@ void setVelocity()
if (stopwatch.isRunning() && (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS)) if (stopwatch.isRunning() && (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS))
{ {
whill.move(vel[0], vel[1], vel[2]); if (b_motor_active) whill.move(vel[0], vel[1], vel[2]);
} }
} }
void sendStatus(WhillOmni::LR lr) void sendStatus()
{ {
while (whill.available(lr)) if (b_streaming)
{ {
packer.pack(HARDWARE_ID, (uint8_t)lr, whill.battery(lr), whill.errorCode(lr)); Serial.println(millis());
packer.pack(HARDWARE_ID, whill.battery(WhillOmni::LR::L), whill.battery(WhillOmni::LR::R), whill.errorCode(WhillOmni::LR::L), whill.errorCode(WhillOmni::LR::R));
XBEE_SERIAL.write(packer.data(), packer.size()); XBEE_SERIAL.write(packer.data(), packer.size());
#ifdef DEBUG_INFO
if (whill.dataset(lr) == 0) printSpeedProfile(lr);
else if (whill.dataset(lr) == 1) printSensorInfo(lr);
else Serial.println("invalid dataset number");
#endif
whill.pop(lr);
} }
} }
......
lib/BorderXBeePacketizer/BorderXBeePacketizer.h
...@@ -83,31 +83,6 @@ public: ...@@ -83,31 +83,6 @@ public:
} }
case WhillXBeeState::DATA: case WhillXBeeState::DATA:
{ {
#if 0
if (data == BorderXBeeProtocol::BYTE_FOOTER)
{
if (buffer.size != 7)
{
Serial.print("recv data size error!!!! 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])
{
calcurateVel();
packets.push(buffer);
}
else
{
++error_count;
}
clearPacket();
break;
}
buffer.data[buffer.size] = data;
++buffer.size;
#else
if ((buffer.id > 0) && (buffer.id < 7)) if ((buffer.id > 0) && (buffer.id < 7))
{ {
if (data == BorderXBeeProtocol::BYTE_FOOTER) if (data == BorderXBeeProtocol::BYTE_FOOTER)
...@@ -165,7 +140,6 @@ public: ...@@ -165,7 +140,6 @@ public:
++error_count; ++error_count;
clearPacket(); clearPacket();
} }
#endif
break; break;
} }
default: default:
...@@ -211,14 +185,15 @@ class BorderXBeePacker ...@@ -211,14 +185,15 @@ class BorderXBeePacker
{ {
public: public:
void pack(uint8_t id, uint8_t lr, uint8_t battery, uint8_t error) void pack(uint8_t id, uint8_t battery_l, uint8_t battery_r, uint8_t error_l, uint8_t error_r)
{ {
buffer[0] = BorderXBeeProtocol::BYTE_HEDADER; buffer[0] = BorderXBeeProtocol::BYTE_HEDADER;
buffer[1] = id; buffer[1] = id;
buffer[2] = lr; buffer[2] = battery_l;
buffer[3] = battery; buffer[3] = battery_r;
buffer[4] = error; buffer[4] = error_l;
buffer[5] = BorderXBeeProtocol::BYTE_FOOTER; buffer[5] = error_r;
buffer[6] = BorderXBeeProtocol::BYTE_FOOTER;
} }
uint8_t* data() { return buffer.data(); } uint8_t* data() { return buffer.data(); }
...@@ -226,7 +201,7 @@ public: ...@@ -226,7 +201,7 @@ public:
private: private:
std::array<uint8_t, 6> buffer; std::array<uint8_t, 7> buffer;
}; };
#endif // BORDERXBEEPACKETIZER_H #endif // BORDERXBEEPACKETIZER_H
lib/Whill/Whill.cpp
...@@ -99,16 +99,85 @@ bool WhillModelC::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_ ...@@ -99,16 +99,85 @@ bool WhillModelC::speed(SPEEDMODE mode, char f_m1, char f_a1, char f_d1, char r_
bool WhillModelC::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode) bool WhillModelC::streaming(bool b, uint16_t interval, DATASET n, SPEEDMODE mode)
{ {
streaming_info.is_streaming = b;
streaming_info.interval = interval;
streaming_info.dataset = n;
streaming_info.speed_mode = mode;
packer.streaming(b, interval, n, mode); packer.streaming(b, interval, n, mode);
return this->send(); return this->send();
} }
void WhillModelC::parse() void WhillModelC::parse()
{ {
static uint32_t prev_ms = millis();
if (streaming_info.is_streaming)
{
if ((millis() - prev_ms) > STATUS_RECEIVE_TIMEOUT)
{
status.info.battery.percent = 0;
status.info.error_code = 253;
}
}
while(serial->available() > 0) while(serial->available() > 0)
{ {
prev_ms = millis();
char data = serial->read(); char data = serial->read();
unpacker.feed(data); unpacker.feed(data);
while (unpacker.available())
{
if (unpacker.dataset() == (uint8_t)DATASET::SPEED_PROFILE)
{
Serial.print("Speed Profile buffer is changed : ");
Serial.println(millis());
#ifdef DEBUG_INFO
printSpeedProfile();
#endif
status.speed.mode = unpacker.speedMode();
status.speed.forward_speed = unpacker.forwardSpeed();
status.speed.forward_accel = unpacker.forwardAccel();
status.speed.forward_decel = unpacker.forwardDecel();
status.speed.reverse_speed = unpacker.reverseSpeed();
status.speed.reverse_accel = unpacker.reverseAccel();
status.speed.reverse_decel = unpacker.reverseDecel();
status.speed.turn_speed = unpacker.turnSpeed();
status.speed.turn_accel = unpacker.turnAccel();
status.speed.turn_decel = unpacker.turnDecel();
}
else if (unpacker.dataset() == (uint8_t)DATASET::SENSOR_INFO)
{
Serial.print("Sensor Info buffer is changed : ");
Serial.println(millis());
#ifdef DEBUG_INFO
printSensorInfo();
#endif
status.info.sensor.accelX = unpacker.sensorAccelX();
status.info.sensor.accelY = unpacker.sensorAccelY();
status.info.sensor.accelZ = unpacker.sensorAccelZ();
status.info.sensor.gyroX = unpacker.sensorGyroX();
status.info.sensor.gyroY = unpacker.sensorGyroY();
status.info.sensor.gyroZ = unpacker.sensorGyroZ();
status.info.joystick.front = unpacker.joystickFront();
status.info.joystick.side = unpacker.joystickSide();
status.info.battery.percent = unpacker.battery();
status.info.battery.current = unpacker.batteryCurrent();
status.info.battery.power_state = unpacker.powerState();
status.info.wheel.speed_mode_indicator = unpacker.speedModeIndicator();
status.info.wheel.angle_rad_r = unpacker.angleRadL();
status.info.wheel.angle_rad_l = unpacker.angleRadR();
status.info.wheel.speed_r = unpacker.speedR();
status.info.wheel.speed_l = unpacker.speedL();
status.info.error_code = unpacker.errorCode();
}
pop();
}
} }
} }
......
lib/Whill/Whill.h
...@@ -16,6 +16,68 @@ class WhillModelC ...@@ -16,6 +16,68 @@ class WhillModelC
float r_m1; // [m/s] float r_m1; // [m/s]
float t_m1; // [m/s]?? TODO: check spec float t_m1; // [m/s]?? TODO: check spec
}; };
// TODO: iranai kamo
// TODO: only struct Status is enough??
struct Status
{
// DATASET::SPEED_PROFILE
struct Speed
{
uint8_t mode;
uint8_t forward_speed;
uint8_t forward_accel;
uint8_t forward_decel;
uint8_t reverse_speed;
uint8_t reverse_accel;
uint8_t reverse_decel;
uint8_t turn_speed;
uint8_t turn_accel;
uint8_t turn_decel;
} speed;
// DATASET::SENSOR_INFO
struct Info
{
struct Sensor
{
uint16_t accelX; // [mg]
uint16_t accelY; // [mg]
uint16_t accelZ; // [mg]
uint16_t gyroX; // [mdps]
uint16_t gyroY; // [mdps]
uint16_t gyroZ; // [mdps]
} sensor;
struct Joystick
{
int8_t front; // -100 - +100
int8_t side; // -100 - +100
} joystick;
struct Battery
{
uint8_t percent; // 0 - 100[%]
uint16_t current; // [mA], sampling rate is 4Hz
bool power_state;
} battery;
struct Wheel
{
uint8_t speed_mode_indicator;
float angle_rad_r; // [rad], range is (-PI, PI)
float angle_rad_l; // [rad], range is (-PI, PI)
float speed_r; // [km/h]
float speed_l; // [km/h]
} wheel;
uint8_t error_code;
} info;
};
struct StreamingInfo
{
bool is_streaming;
uint16_t interval;
DATASET dataset;
SPEEDMODE speed_mode;
};
public: public:
...@@ -33,6 +95,7 @@ public: ...@@ -33,6 +95,7 @@ public:
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);
bool streaming(bool b, uint16_t interval = 1000, DATASET n = DATASET::SENSOR_INFO, SPEEDMODE mode = SPEEDMODE::RS232C); bool streaming(bool b, uint16_t interval = 1000, DATASET n = DATASET::SENSOR_INFO, SPEEDMODE mode = SPEEDMODE::RS232C);
uint16_t streamingInterval() { return streaming_info.interval; }
void parse(); void parse();
const uint8_t available() const { return unpacker.available(); } const uint8_t available() const { return unpacker.available(); }
...@@ -41,39 +104,36 @@ public: ...@@ -41,39 +104,36 @@ public:
const char* const data() const { return unpacker.data(); } const char* const data() const { return unpacker.data(); }
void pop() { unpacker.pop(); } void pop() { unpacker.pop(); }
// DATASET Number
uint8_t dataset() { return unpacker.dataset(); }
// DATASET::SPEED_PROFILE // DATASET::SPEED_PROFILE
uint8_t speedMode() { return unpacker.speedMode(); } uint8_t speedMode() { return status.speed.mode; }
uint8_t forwardSpeed() { return unpacker.forwardSpeed(); } uint8_t forwardSpeed() { return status.speed.forward_speed; }
uint8_t forwardAccel() { return unpacker.forwardAccel(); } uint8_t forwardAccel() { return status.speed.forward_accel; }
uint8_t forwardDecel() { return unpacker.forwardDecel(); } uint8_t forwardDecel() { return status.speed.forward_decel; }
uint8_t reverseSpeed() { return unpacker.reverseSpeed(); } uint8_t reverseSpeed() { return status.speed.reverse_speed; }
uint8_t reverseAccel() { return unpacker.reverseAccel(); } uint8_t reverseAccel() { return status.speed.reverse_accel; }
uint8_t reverseDecel() { return unpacker.reverseDecel(); } uint8_t reverseDecel() { return status.speed.reverse_decel; }
uint8_t turnSpeed() { return unpacker.turnSpeed(); } uint8_t turnSpeed() { return status.speed.turn_speed; }
uint8_t turnAccel() { return unpacker.turnAccel(); } uint8_t turnAccel() { return status.speed.turn_accel; }
uint8_t turnDecel() { return unpacker.turnDecel(); } uint8_t turnDecel() { return status.speed.turn_decel; }
// DATASET::SENSOR_INFO // DATASET::SENSOR_INFO
uint16_t sensorAccelX() { return unpacker.sensorAccelX(); } // [mg] uint16_t sensorAccelX() { return status.info.sensor.accelX; } // [mg]
uint16_t sensorAccelY() { return unpacker.sensorAccelY(); } // [mg] uint16_t sensorAccelY() { return status.info.sensor.accelY; } // [mg]
uint16_t sensorAccelZ() { return unpacker.sensorAccelZ(); } // [mg] uint16_t sensorAccelZ() { return status.info.sensor.accelZ; } // [mg]
uint16_t sensorGyroX() { return unpacker.sensorGyroX(); } // [mdps] uint16_t sensorGyroX() { return status.info.sensor.gyroX; } // [mdps]
uint16_t sensorGyroY() { return unpacker.sensorGyroY(); } // [mdps] uint16_t sensorGyroY() { return status.info.sensor.gyroY; } // [mdps]
uint16_t sensorGyroZ() { return unpacker.sensorGyroZ(); } // [mdps] uint16_t sensorGyroZ() { return status.info.sensor.gyroZ; } // [mdps]
int8_t joystickFront() { return unpacker.joystickFront(); } // -100 - +100 int8_t joystickFront() { return status.info.joystick.front; } // -100 - +100
int8_t joystickSide() { return unpacker.joystickSide(); } // -100 - +100 int8_t joystickSide() { return status.info.joystick.side; } // -100 - +100
uint8_t battery() { return unpacker.battery(); } // 0 - 100[%] uint8_t battery() { return status.info.battery.percent; } // 0 - 100[%]
int16_t batteryCurrent() { return unpacker.batteryCurrent(); } // [mA], sampling rate is 4Hz int16_t batteryCurrent() { return status.info.battery.current; } // [mA], sampling rate is 4Hz
float angleRadR() { return unpacker.angleRadR(); } // [rad], range is (-PI, PI) float angleRadR() { return status.info.wheel.angle_rad_r; } // [rad], range is (-PI, PI)
float angleRadL() { return unpacker.angleRadL(); } // [rad], range is (-PI, PI) float angleRadL() { return status.info.wheel.angle_rad_l; } // [rad], range is (-PI, PI)
float speedR() { return unpacker.speedR(); } // [km/h] float speedR() { return status.info.wheel.speed_r; } // [km/h]
float speedL() { return unpacker.speedL(); } // [km/h] float speedL() { return status.info.wheel.speed_l; } // [km/h]
bool powerState() { return unpacker.powerState(); } bool powerState() { return status.info.battery.power_state; }
uint8_t speedModeIndicator() { return unpacker.speedModeIndicator(); } uint8_t speedModeIndicator() { return status.info.wheel.speed_mode_indicator; }
uint8_t errorCode() { return unpacker.errorCode(); } uint8_t errorCode() { return status.info.error_code; }
// 34 バッテリー通信エラー // 34 バッテリー通信エラー
// 61 モーター高温 // 61 モーター高温
...@@ -87,10 +147,14 @@ private: ...@@ -87,10 +147,14 @@ private:
bool send(); bool send();
const uint32_t STATUS_RECEIVE_TIMEOUT = 2000;
Stream* serial; Stream* serial;
Packer packer; Packer packer;
Unpacker unpacker; Unpacker unpacker;
SpeedProfile spd_profile; SpeedProfile spd_profile;
Status status;
StreamingInfo streaming_info;
}; };
WHILL_NAMESPACE_END WHILL_NAMESPACE_END
......
lib/Whill/WhillOmni.h
...@@ -38,9 +38,6 @@ public: ...@@ -38,9 +38,6 @@ public:
void pop(LR lr) { whills[(uint8_t)lr].pop(); } void pop(LR lr) { whills[(uint8_t)lr].pop(); }
void pop() { for (auto& w : whills) w.pop(); } void pop() { for (auto& w : whills) w.pop(); }
// DATASET Number
uint8_t dataset(LR lr) { return whills[(uint8_t)lr].dataset(); }
// DATASET::SPEED_PROFILE // DATASET::SPEED_PROFILE
uint8_t speedMode(LR lr) { return whills[(uint8_t)lr].speedMode(); } uint8_t speedMode(LR lr) { return whills[(uint8_t)lr].speedMode(); }
uint8_t forwardSpeed(LR lr) { return whills[(uint8_t)lr].forwardSpeed(); } uint8_t forwardSpeed(LR lr) { return whills[(uint8_t)lr].forwardSpeed(); }
......
src/main.cpp
...@@ -4,6 +4,8 @@ ...@@ -4,6 +4,8 @@
#include "global.h" #include "global.h"
#include "main.h" #include "main.h"
void disableMotor() { b_motor_active = false; }
void setup() void setup()
{ {
Serial.begin(115200); Serial.begin(115200);
...@@ -24,7 +26,7 @@ void setup() ...@@ -24,7 +26,7 @@ void setup()
// ----- Emergency Switch ----- // ----- Emergency Switch -----
pinMode(32, INPUT_PULLUP); pinMode(PIN_EMERGENCY, INPUT_PULLUP);
// ----- check I2C ----- // ----- check I2C -----
Wire.begin(); Wire.begin();
...@@ -36,8 +38,8 @@ void setup() ...@@ -36,8 +38,8 @@ void setup()
// ----- check LED Driver ----- // ----- check LED Driver -----
Serial.println("IR LED setup"); Serial.println("IR LED setup");
irled.setup(Wire); irled.setup(Wire);
pinMode(17, OUTPUT); pinMode(PIN_IRLED_OE, OUTPUT);
digitalWrite(17, LOW); // Output Enable digitalWrite(PIN_IRLED_OE, LOW); // Output Enable
WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L
...@@ -53,19 +55,20 @@ void setup() ...@@ -53,19 +55,20 @@ void setup()
Serial.println("set speed profile"); 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
delay(1000); delay(500);
Serial.println("check speed profile"); Serial.println("check speed profile");
whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SPEED_PROFILE); // valid in next streaming phase 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::L, WHILL_INFO_INTERVAL_MS + 500);
waitReply(WhillOmni::LR::R); waitReply(WhillOmni::LR::R);
Serial.println("change streaming to sensor info"); 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(500);
attachInterrupt(digitalPinToInterrupt(PIN_EMERGENCY), disableMotor, FALLING);
Serial.println("start program"); Serial.println("start program");
} }
...@@ -76,6 +79,11 @@ void loop() ...@@ -76,6 +79,11 @@ void loop()
setVelocity(); setVelocity();
whill.parse(); whill.parse();
sendStatus(WhillOmni::LR::L);
sendStatus(WhillOmni::LR::R); static uint32_t prev_ms = millis();
if ((millis() - prev_ms) > streaming_interval)
{
sendStatus();
prev_ms = millis();
}
} }
  • 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