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;
static constexpr uint32_t XBEE_SERIAL_BAUD = 38400;
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;
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
{
uint8_t channel;
......@@ -24,7 +31,6 @@ struct XBeeSettings
uint32_t dst_addr_l;
uint8_t packetization_timeout;
};
XBeeSettings xb_settings[6];
#endif // WHILL_GLOBAL_H
include/main.h
......@@ -25,7 +25,6 @@ StopWatch stopwatch;
void printSpeedProfile(WhillOmni::LR lr)
{
Serial.println("SPEED_PROFILE");
Serial.print("dataset : "); Serial.println(whill.dataset(lr));
Serial.print("mode : "); Serial.println(whill.speedMode(lr));
Serial.print("fwd spd : "); Serial.println(whill.forwardSpeed(lr));
Serial.print("fwd acc : "); Serial.println(whill.forwardAccel(lr));
......@@ -239,11 +238,12 @@ void setVelocity()
if (status_report_duration == 0xFFFFFFFF)
{
whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
b_streaming = false;
}
else
{
whill.streaming(true, status_report_duration, DATASET::SENSOR_INFO);
b_streaming = true;
streaming_interval = status_report_duration;
}
}
......@@ -261,23 +261,17 @@ void setVelocity()
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());
#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:
}
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 (data == BorderXBeeProtocol::BYTE_FOOTER)
......@@ -165,7 +140,6 @@ public:
++error_count;
clearPacket();
}
#endif
break;
}
default:
......@@ -211,14 +185,15 @@ class BorderXBeePacker
{
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[1] = id;
buffer[2] = lr;
buffer[3] = battery;
buffer[4] = error;
buffer[5] = BorderXBeeProtocol::BYTE_FOOTER;
buffer[2] = battery_l;
buffer[3] = battery_r;
buffer[4] = error_l;
buffer[5] = error_r;
buffer[6] = BorderXBeeProtocol::BYTE_FOOTER;
}
uint8_t* data() { return buffer.data(); }
......@@ -226,7 +201,7 @@ public:
private:
std::array<uint8_t, 6> buffer;
std::array<uint8_t, 7> buffer;
};
#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_
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);
return this->send();
}
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)
{
prev_ms = millis();
char data = serial->read();
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
float r_m1; // [m/s]
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:
......@@ -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 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();
const uint8_t available() const { return unpacker.available(); }
......@@ -41,39 +104,36 @@ public:
const char* const data() const { return unpacker.data(); }
void pop() { unpacker.pop(); }
// DATASET Number
uint8_t dataset() { return unpacker.dataset(); }
// DATASET::SPEED_PROFILE
uint8_t speedMode() { return unpacker.speedMode(); }
uint8_t forwardSpeed() { return unpacker.forwardSpeed(); }
uint8_t forwardAccel() { return unpacker.forwardAccel(); }
uint8_t forwardDecel() { return unpacker.forwardDecel(); }
uint8_t reverseSpeed() { return unpacker.reverseSpeed(); }
uint8_t reverseAccel() { return unpacker.reverseAccel(); }
uint8_t reverseDecel() { return unpacker.reverseDecel(); }
uint8_t turnSpeed() { return unpacker.turnSpeed(); }
uint8_t turnAccel() { return unpacker.turnAccel(); }
uint8_t turnDecel() { return unpacker.turnDecel(); }
uint8_t speedMode() { return status.speed.mode; }
uint8_t forwardSpeed() { return status.speed.forward_speed; }
uint8_t forwardAccel() { return status.speed.forward_accel; }
uint8_t forwardDecel() { return status.speed.forward_decel; }
uint8_t reverseSpeed() { return status.speed.reverse_speed; }
uint8_t reverseAccel() { return status.speed.reverse_accel; }
uint8_t reverseDecel() { return status.speed.reverse_decel; }
uint8_t turnSpeed() { return status.speed.turn_speed; }
uint8_t turnAccel() { return status.speed.turn_accel; }
uint8_t turnDecel() { return status.speed.turn_decel; }
// DATASET::SENSOR_INFO
uint16_t sensorAccelX() { return unpacker.sensorAccelX(); } // [mg]
uint16_t sensorAccelY() { return unpacker.sensorAccelY(); } // [mg]
uint16_t sensorAccelZ() { return unpacker.sensorAccelZ(); } // [mg]
uint16_t sensorGyroX() { return unpacker.sensorGyroX(); } // [mdps]
uint16_t sensorGyroY() { return unpacker.sensorGyroY(); } // [mdps]
uint16_t sensorGyroZ() { return unpacker.sensorGyroZ(); } // [mdps]
int8_t joystickFront() { return unpacker.joystickFront(); } // -100 - +100
int8_t joystickSide() { return unpacker.joystickSide(); } // -100 - +100
uint8_t battery() { return unpacker.battery(); } // 0 - 100[%]
int16_t batteryCurrent() { return unpacker.batteryCurrent(); } // [mA], sampling rate is 4Hz
float angleRadR() { return unpacker.angleRadR(); } // [rad], range is (-PI, PI)
float angleRadL() { return unpacker.angleRadL(); } // [rad], range is (-PI, PI)
float speedR() { return unpacker.speedR(); } // [km/h]
float speedL() { return unpacker.speedL(); } // [km/h]
bool powerState() { return unpacker.powerState(); }
uint8_t speedModeIndicator() { return unpacker.speedModeIndicator(); }
uint8_t errorCode() { return unpacker.errorCode(); }
uint16_t sensorAccelX() { return status.info.sensor.accelX; } // [mg]
uint16_t sensorAccelY() { return status.info.sensor.accelY; } // [mg]
uint16_t sensorAccelZ() { return status.info.sensor.accelZ; } // [mg]
uint16_t sensorGyroX() { return status.info.sensor.gyroX; } // [mdps]
uint16_t sensorGyroY() { return status.info.sensor.gyroY; } // [mdps]
uint16_t sensorGyroZ() { return status.info.sensor.gyroZ; } // [mdps]
int8_t joystickFront() { return status.info.joystick.front; } // -100 - +100
int8_t joystickSide() { return status.info.joystick.side; } // -100 - +100
uint8_t battery() { return status.info.battery.percent; } // 0 - 100[%]
int16_t batteryCurrent() { return status.info.battery.current; } // [mA], sampling rate is 4Hz
float angleRadR() { return status.info.wheel.angle_rad_r; } // [rad], range is (-PI, PI)
float angleRadL() { return status.info.wheel.angle_rad_l; } // [rad], range is (-PI, PI)
float speedR() { return status.info.wheel.speed_r; } // [km/h]
float speedL() { return status.info.wheel.speed_l; } // [km/h]
bool powerState() { return status.info.battery.power_state; }
uint8_t speedModeIndicator() { return status.info.wheel.speed_mode_indicator; }
uint8_t errorCode() { return status.info.error_code; }
// 34 バッテリー通信エラー
// 61 モーター高温
......@@ -87,10 +147,14 @@ private:
bool send();
const uint32_t STATUS_RECEIVE_TIMEOUT = 2000;
Stream* serial;
Packer packer;
Unpacker unpacker;
SpeedProfile spd_profile;
Status status;
StreamingInfo streaming_info;
};
WHILL_NAMESPACE_END
......
lib/Whill/WhillOmni.h
......@@ -38,9 +38,6 @@ public:
void pop(LR lr) { whills[(uint8_t)lr].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
uint8_t speedMode(LR lr) { return whills[(uint8_t)lr].speedMode(); }
uint8_t forwardSpeed(LR lr) { return whills[(uint8_t)lr].forwardSpeed(); }
......
src/main.cpp
......@@ -4,6 +4,8 @@
#include "global.h"
#include "main.h"
void disableMotor() { b_motor_active = false; }
void setup()
{
Serial.begin(115200);
......@@ -24,7 +26,7 @@ void setup()
// ----- Emergency Switch -----
pinMode(32, INPUT_PULLUP);
pinMode(PIN_EMERGENCY, INPUT_PULLUP);
// ----- check I2C -----
Wire.begin();
......@@ -36,8 +38,8 @@ void setup()
// ----- check LED Driver -----
Serial.println("IR LED setup");
irled.setup(Wire);
pinMode(17, OUTPUT);
digitalWrite(17, LOW); // Output Enable
pinMode(PIN_IRLED_OE, OUTPUT);
digitalWrite(PIN_IRLED_OE, LOW); // Output Enable
WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L
......@@ -53,19 +55,20 @@ void setup()
Serial.println("set speed profile");
whill.speed(SPEEDMODE::RS232C, 60, 90, 160, 60, 50, 80, 60, 60, 160); // max
delay(1000);
delay(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::L, WHILL_INFO_INTERVAL_MS + 500);
waitReply(WhillOmni::LR::R);
Serial.println("change streaming to 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");
}
......@@ -76,6 +79,11 @@ void loop()
setVelocity();
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