Skip to content

Hideaki Tai / omni_main

Go to a project
Commit c4396fe9 by Hideaki Tai
Options

test ok

parent 907a3c2f
Inline Side-by-side
Showing with 809 additions and 77 deletions
include/global.h
......@@ -14,4 +14,18 @@ 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.;
uint8_t HARDWARE_ID = 0xFF;
bool b_need_xbee_init = false;
struct XBeeSettings
{
uint8_t channel;
uint16_t pan_id;
uint32_t dst_addr_h;
uint32_t dst_addr_l;
uint8_t packetization_timeout;
};
XBeeSettings xb_settings[6];
#endif // WHILL_GLOBAL_H
include/main.h
......@@ -6,12 +6,20 @@
#include <WhillOmni.h>
#include <BorderXBeePacketizer.h>
#include <StopWatch.h>
#include <XBeeATCmds.h>
#include <MPU9250.h>
#include <PCA9624.h>
#include <RzmEepromMap.h>
using namespace Whill;
WhillOmni whill;
BorderXBeePacker packer;
BorderXBeeUnpacker unpacker;
XBeeATCmds xbeecmd;
MPU9250 mpu;
PCA9624 irled(0x60);
RzmEepromMap eepmap;
StopWatch stopwatch;
void printSpeedProfile(WhillOmni::LR lr)
......@@ -52,6 +60,174 @@ void printSensorInfo(WhillOmni::LR lr)
Serial.print("error code : "); Serial.println(whill.errorCode(lr));
}
void identify()
{
pinMode(24, INPUT_PULLUP);
pinMode(25, INPUT_PULLUP);
pinMode(26, INPUT_PULLUP);
pinMode(27, INPUT_PULLUP);
Serial.print("bit 0 : "); Serial.println(digitalRead(27));
Serial.print("bit 1 : "); Serial.println(digitalRead(26));
Serial.print("bit 2 : "); Serial.println(digitalRead(25));
Serial.print("bit 3 : "); Serial.println(digitalRead(24));
HARDWARE_ID = (!digitalRead(24) << 3) | (!digitalRead(25) << 2) | (!digitalRead(26) << 1) | (!digitalRead(27) << 0);
Serial.print("ID = "); Serial.println(HARDWARE_ID);
}
bool initEeprom(uint8_t hw_id)
{
if (eepmap.checkHardwareID(hw_id))
{
Serial.println("EEPROM HW ID Check OK");
return true;
}
else
{
Serial.print("EEPROM HW ID is different!! Writing Current ID = ");
Serial.println(hw_id);
eepmap.hardwareID(hw_id);
b_need_xbee_init = true;
}
return false;
}
void setupXBee(uint8_t hw_id)
{
xb_settings[0].channel = 0x12;
xb_settings[0].pan_id = 0x07;
xb_settings[0].dst_addr_h = 0x00000000;
xb_settings[0].dst_addr_l = 0x0000FFFF;
xb_settings[0].packetization_timeout = 10;
xb_settings[1].channel = 0x13;
xb_settings[1].pan_id = 0x08;
xb_settings[1].dst_addr_h = 0x00000000;
xb_settings[1].dst_addr_l = 0x0000FFFF;
xb_settings[1].packetization_timeout = 10;
xb_settings[2].channel = 0x14;
xb_settings[2].pan_id = 0x09;
xb_settings[2].dst_addr_h = 0x00000000;
xb_settings[2].dst_addr_l = 0x0000FFFF;
xb_settings[2].packetization_timeout = 10;
xb_settings[3].channel = 0x15;
xb_settings[3].pan_id = 0x0A;
xb_settings[3].dst_addr_h = 0x00000000;
xb_settings[3].dst_addr_l = 0x0000FFFF;
xb_settings[3].packetization_timeout = 10;
xb_settings[4].channel = 0x16;
xb_settings[4].pan_id = 0x0B;
xb_settings[4].dst_addr_h = 0x00000000;
xb_settings[4].dst_addr_l = 0x0000FFFF;
xb_settings[4].packetization_timeout = 10;
xb_settings[5].channel = 0x17;
xb_settings[5].pan_id = 0x0C;
xb_settings[5].dst_addr_h = 0x00000000;
xb_settings[5].dst_addr_l = 0x0000FFFF;
xb_settings[5].packetization_timeout = 10;
uint8_t index = hw_id;
if (hw_id > 5) index = 0;
xbeecmd.channel(xb_settings[index].channel);
xbeecmd.panID(xb_settings[index].pan_id);
xbeecmd.dstAddrH(xb_settings[index].dst_addr_h);
xbeecmd.dstAddrL(xb_settings[index].dst_addr_l);
xbeecmd.baudrate(0x5);
xbeecmd.packtizationTimeout(xb_settings[index].packetization_timeout);
xbeecmd.save();
xbeecmd.apply();
xbeecmd.reset();
// xbeecmd.exit();
XBEE_SERIAL.end();
delay(1000);
XBEE_SERIAL.begin(XBEE_SERIAL_BAUD);
xbeecmd.enter();
}
void initXBee(uint8_t hw_id)
{
XBEE_SERIAL.begin(XBEE_SERIAL_BAUD);
xbeecmd.attach(XBEE_SERIAL);
if (!b_need_xbee_init)
{
return; // no communication check
// // check XBee Connection
// if (xbeecmd.enter())
// {
// if (xbeecmd.ack())
// {
// Serial.println("XBee Communication Check OK");
// xbeecmd.exit();
// return;
// }
// else
// {
// Serial.println("XBee Communication ERROR !! (NO ACK)");
// b_need_xbee_init = true;
// }
// }
// else
// {
// Serial.println("XBee Communication ERROR !! (CANNOT ENTER)");
// b_need_xbee_init = true;
// }
}
else
{
Serial.println("ID has been changed!! Re-Init XBee...");
b_need_xbee_init = true;
}
// XBee initialize process
XBEE_SERIAL.end();
XBEE_SERIAL.begin(9600);
if (!xbeecmd.enter())
{
Serial.println("XBee baudrate != 9600. Retry in default baudrate...");
XBEE_SERIAL.end();
XBEE_SERIAL.begin(XBEE_SERIAL_BAUD);
if (!xbeecmd.enter())
{
while(1)
{
Serial.println("XBee Communication Error!!! Check Connection w/ XBee...");
static bool led = false;
digitalWrite(13, led);
led = !led;
delay(1000);
}
}
}
else
{
Serial.println("XBee baudrate == 9600. Initilize XBee");
b_need_xbee_init = true;
}
if (b_need_xbee_init) setupXBee(hw_id);
xbeecmd.firmwareVer();
xbeecmd.hardwareVer();
Serial.println("XBee Ready");
xbeecmd.exit();
}
void setVelocity()
{
#if 0
......@@ -68,6 +244,21 @@ void setVelocity()
static float vel[3];
unpacker.parse();
if (unpacker.isStatusReportChanged())
{
uint32_t status_report_duration = unpacker.statusReportDurationMs();
if (status_report_duration == 0xFFFFFFFF)
{
whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
}
else
{
whill.streaming(true, status_report_duration, DATASET::SENSOR_INFO);
}
}
while (unpacker.available())
{
if (unpacker.id() == MY_ID)
......@@ -78,18 +269,15 @@ void setVelocity()
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.isRunning() && (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS))
{
whill.move(vel[0], vel[1], vel[2]);
}
else
{
}
if (stopwatch.ms() < XBEE_COMMAND_TIMEOUT_MS) whill.move(vel[0], vel[1], vel[2]);
#endif
}
......@@ -125,4 +313,36 @@ void waitReply(WhillOmni::LR lr, uint16_t wait_ms = 0)
}
}
template <typename WireType>
void i2c_Scan(WireType& wire)
{
Serial.println ();
Serial.println ("I2C scanner. Scanning ...");
byte count = 0;
wire.begin();
for (byte i = 8; i < 120; i++)
{
wire.beginTransmission (i);
if (wire.endTransmission () == 0)
{
Serial.print ("Found address: ");
Serial.print (i, DEC);
Serial.print (" (0x");
Serial.print (i, HEX);
Serial.println (")");
count++;
delay (1);
}
}
Serial.println ("Done.");
Serial.print ("Found ");
Serial.print (count, DEC);
Serial.println (" device(s).");
Serial.println ("***********");
Serial.println (" ");
}
#endif // MAIN_HEADER_H
lib/BorderXBeePacketizer/BorderXBeePacketizer.h
......@@ -41,6 +41,13 @@ public:
float velocityY() { return packets.front().vel[1]; }
float velocityW() { return packets.front().vel[2]; }
bool isStatusReportChanged()
{
bool b = b_status_report_changed;
b_status_report_changed = false;
return b;
}
uint32_t statusReportDurationMs()
{
if (status_report_duration == 1) return 50;
......@@ -143,6 +150,7 @@ public:
else
{
status_report_duration = buffer.data[0];
b_status_report_changed = true;
}
clearPacket();
break;
......@@ -193,6 +201,8 @@ private:
std::queue<WhillXBeeData> packets;
WhillXBeeState state;
uint32_t error_count;
bool b_status_report_changed {false};
uint8_t status_report_duration;
};
......
MPU9250 @ 56ae8a62
Subproject commit 56ae8a621da86afd88bba51a9d8832246ce8ea20
lib/PCA9624/PCA9624.h 0 → 100644
#pragma once
#ifndef PCA9624_H
#define PCA9624_H
#ifdef TEENSYDUINO
#include <i2c_t3.h>
#else
// #include <Wire.h>
#endif
template <typename WireType>
class PCA9624_
{
const uint8_t ADDR;
enum class Reg
{
MODE1,
MODE2,
PWM0,
PWM1,
PWM2,
PWM3,
PWM4,
PWM5,
PWM6,
PWM7,
GRPPWM,
GRPFREQ,
LEDOUT0,
LEDOUT1,
SUBADR1,
SUBADR2,
SUBADR3,
ALLCALLADR
};
public:
enum class LEDState
{
Disable, // LEDs are OFF
FullyOn, // uncontrollable, fully on
PWM_EACH, // controllable from PWMx
PWM_EACH_AND_GROUP // controllable from both PWMx and GRPPWM
};
PCA9624_(uint8_t addr) : ADDR(addr) {}
void attach(WireType& w) { wire = &w; }
void setup(WireType& w)
{
attach(w);
sleep(false);
setDriverOutputState(LEDState::PWM_EACH_AND_GROUP);
}
void sleep(bool b)
{
uint8_t data;
if (b) data = 0b10010001;
else data = 0b10000001;
writeByte(Reg::MODE1, data);
}
void setDriverOutputState(LEDState state)
{
uint8_t data[2];
data[0] = ((uint8_t)state << 6) | ((uint8_t)state << 4) | ((uint8_t)state << 2) | ((uint8_t)state << 0);
data[1] = ((uint8_t)state << 6) | ((uint8_t)state << 4) | ((uint8_t)state << 2) | ((uint8_t)state << 0);
writeBytes(Reg::LEDOUT0, data, sizeof(data));
}
void drive(uint8_t ch, uint8_t vol)
{
writeByte(ADDR, (uint8_t)Reg::PWM0 + ch, vol);
}
void drive(uint8_t vol)
{
uint8_t data[8];
for (uint8_t i = 0; i < 8; ++i) data[i] = vol;
writeBytes(Reg::PWM0, data, sizeof(data));
}
// void drive() // TODO: variable arguments
// {
// }
private:
void writeByte(Reg reg, uint8_t data)
{
writeByte(ADDR, (uint8_t)reg, data);
}
void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
wire->beginTransmission(address); // Initialize the Tx buffer
wire->write(subAddress); // Put slave register address in Tx buffer
wire->write(data); // Put data in Tx buffer
i2c_err_ = wire->endTransmission(); // Send the Tx buffer
if (i2c_err_) pirntI2CError();
}
void writeBytes(Reg reg, uint8_t* data, uint8_t size)
{
writeBytes(ADDR, (uint8_t)reg, data, size);
}
void writeBytes(uint8_t address, uint8_t subAddress, uint8_t* data, uint8_t size)
{
wire->beginTransmission(address); // Initialize the Tx buffer
wire->write(subAddress | 0x80); // auto increment
for (uint8_t i = 0; i < size; ++i)
wire->write(*(data + i)); // Put data in Tx buffer
i2c_err_ = wire->endTransmission(); // Send the Tx buffer
if (i2c_err_) pirntI2CError();
}
uint8_t readByte(uint8_t address, uint8_t subAddress)
{
uint8_t data = 0; // `data` will store the register data
wire->beginTransmission(address); // Initialize the Tx buffer
wire->write(subAddress); // Put slave register address in Tx buffer
i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
if (i2c_err_) pirntI2CError();
wire->requestFrom(address, (size_t)1); // Read one byte from slave register address
if (wire->available()) data = wire->read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}
void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{
wire->beginTransmission(address); // Initialize the Tx buffer
wire->write(subAddress); // Put slave register address in Tx buffer
i2c_err_ = wire->endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
if (i2c_err_) pirntI2CError();
uint8_t i = 0;
wire->requestFrom(address, count); // Read bytes from slave register address
while (wire->available())
{
dest[i++] = wire->read();
} // Put read results in the Rx buffer
}
void pirntI2CError()
{
if (i2c_err_ == 7) return; // to avoid stickbreaker-i2c branch's error code
Serial.print("I2C ERROR CODE : ");
Serial.println(i2c_err_);
}
std::array<uint8_t, 8> leds;
WireType* wire;
uint8_t i2c_err_;
};
#ifdef TEENSYDUINO
using PCA9624 = PCA9624_<i2c_t3>;
#else
using PCA9624 = PCA9624_<TwoWire>;
#endif
#endif // PCA9624_H
lib/RzmEepromMap/RzmEepromMap.h 0 → 100644
#pragma once
#ifndef RZMEEPROMMAP_H
#define RZMEEPROMMAP_H
#include <Arduino.h>
#include <EEPROM.h>
class RzmEepromMap
{
enum class EepromMapAddr
{
FW_VERSION_MAJOR = 0x00,
FW_VERSION_MINOR = 0x01,
HARDWARE_ID = 0x02
};
public:
uint16_t firmwareVersion(uint8_t major = 0xFF, uint8_t minor = 0xFF)
{
if ((major == 0xFF) && (minor == 0xFF))
{
Serial.print((EEPROM.read((uint16_t)EepromMapAddr::FW_VERSION_MAJOR)));
Serial.print(".");
Serial.print((EEPROM.read((uint16_t)EepromMapAddr::FW_VERSION_MINOR)));
Serial.println();
return (uint16_t)((EEPROM.read((uint16_t)EepromMapAddr::FW_VERSION_MAJOR) << 8) | (EEPROM.read((uint16_t)EepromMapAddr::FW_VERSION_MINOR) << 0));
}
else
{
EEPROM.write((uint16_t)EepromMapAddr::FW_VERSION_MAJOR, major);
EEPROM.write((uint16_t)EepromMapAddr::FW_VERSION_MINOR, minor);
return 0;
}
}
uint8_t hardwareID(uint8_t i = 0xFF)
{
if (i == 0xFF)
{
return EEPROM.read((uint16_t)EepromMapAddr::HARDWARE_ID);
}
else
{
EEPROM.write((uint16_t)EepromMapAddr::HARDWARE_ID, i);
return 0;
}
}
bool checkHardwareID(uint8_t i)
{
return (hardwareID() == i);
}
private:
};
#endif // RZMEEPROMMAP_H
lib/Whill/OmniModel.h
......@@ -23,7 +23,7 @@ namespace Work
const double R = 0.12; // wheel radius [m]
// 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 L = 0.435; // from center to wheel [m]
// const double T = 0.29; // tread / 2.0 [m] (from center to wheel)
const double Mp = 10.0; // mass of platform [kg]
const double Ml = 10.0; // mass of load [kg]
......
lib/XBeeATCmds/XBeeATCmds.h
#pragma once
#ifndef XBEEATCMDS_H
#define XBEEATCMDS_H
namespace XBeeATCmdsUtil
{
template <size_t size>
struct same_size_int;
template <size_t size>
using same_size_int_t = typename same_size_int<size>::type;
template <> struct same_size_int<1> { using type = int8_t; };
template <> struct same_size_int<2> { using type = int16_t; };
template <> struct same_size_int<4> { using type = int32_t; };
template <> struct same_size_int<8> { using type = int64_t; };
template <typename T, typename std::enable_if<std::is_floating_point<T>::value>::type* = nullptr>
struct IntFloatUnion_impl {
using type = union {
same_size_int_t<sizeof(T)> x;
T f;
};
};
template <typename T>
using IntFloatUnion = typename IntFloatUnion_impl<T>::type;
template <typename T, size_t length = sizeof(T) * 2>
auto toHex(T value)
-> typename std::enable_if<std::is_integral<T>::value, String>::type
{
String format = "%0" + String(length) + "X";
char hex[length + 1];
sprintf(hex, format.c_str(), value);
return String(hex);
}
template <typename T, size_t length = sizeof(T) * 2>
auto toHex(T value)
-> typename std::enable_if<std::is_floating_point<T>::value, String>::type
{
IntFloatUnion<T> myUnion;
myUnion.f = value;
return toHex(myUnion.x);
}
}
class XBeeATCmds
{
public:
void attach(Stream& s)
{
serial = &s;
}
bool applyChange()
// ----- enter AT Command mode -----
bool enter()
{
Serial.print("ENTER COMMAND MODE: ");
serial->print("+++");
delay(WAIT_COMMAND_MODE_MS);
return dumpReply();
}
bool ack()
{
Serial.print("ACK: ");
serial->println("AT");
return dumpReply();
}
// ----- special commands -----
bool save()
{
Serial.print("WRITE DATA TO MEMORY: ");
serial->println("ATWR");
return dumpReply();
}
bool restore()
{
Serial.printf("APPLY CHANGE: \n");
serial->printf("ATAC\r");
Serial.print("RESTORE DATA TO FACTORY DEFAULTS: ");
serial->println("ATRE");
return dumpReply();
}
bool reset()
{
Serial.printf("RESET: \n");
serial->printf("ATFR\r");
Serial.print("RESET: ");
serial->println("ATFR");
return dumpReply();
}
// ----- network setting commands -----
bool channel(uint8_t ch = 0xFF)
{
Serial.print("CHANNEL: ");
serial->print("ATCH");
if (ch == 0xFF) serial->println();
else serial->println(XBeeATCmdsUtil::toHex(ch));
return dumpReply();
}
bool panID(uint16_t pan = 0xFFFF)
{
Serial.print("PAN ID: ");
serial->print("ATID");
serial->println(XBeeATCmdsUtil::toHex(pan));
return dumpReply();
}
bool dstAddrH(uint32_t addr)
{
Serial.print("DESTINATION ADDR H: ");
serial->print("ATDH");
serial->println(XBeeATCmdsUtil::toHex(addr));
return dumpReply();
}
bool dstAddrL(uint32_t addr)
{
Serial.print("DESTINATION ADDR L: ");
serial->print("ATDL");
serial->println(XBeeATCmdsUtil::toHex(addr));
return dumpReply();
}
bool sourceAddr(uint16_t addr)
{
Serial.print("DESTINATION ADDR L: ");
serial->print("ATMY");
serial->println(XBeeATCmdsUtil::toHex(addr));
return dumpReply();
}
bool writeToMemory()
bool retries(uint8_t times)
{
Serial.print("XBEE RETRIES: ");
serial->print("ATRR");
serial->println(XBeeATCmdsUtil::toHex(times));
return dumpReply();
}
bool coordinator(bool b)
{
Serial.print("COORDINATOR ENABLE: ");
serial->print("ATCE");
serial->println(XBeeATCmdsUtil::toHex((uint8_t)b));
return dumpReply();
}
// SH : Serial Number High
// SL : Serial Number Low
// RN : Random Delay Slots
// MM : MAC Mode
// NI : Node Identifier
// ND : Node Discover
// NT : Node Discover Time
// NO : Node Discovery Options
// DN : Destination Node
// SC : Scan Channel
// SD : Scan Duration
// A1 : End Device Association
// A2 : Coordinator Association
// AI : Association Indication
// DA : Force Disassociation
// FP : Force Poll
// AS : Active Scan
// ED : Energy Scan
// EE : AES Encryption Enable
// KY : AES Encryption Key
// ----- RF Interfacing Commands -----
bool powerLevel(uint8_t lvl)
{
Serial.printf("WRITE TO MEMORY: \n");
serial->printf("ATWR\r");
Serial.print("POWER LEVEL: ");
serial->print("ATPL");
serial->println(XBeeATCmdsUtil::toHex(lvl));
return dumpReply();
}
bool setChannelMask(uint64_t mask)
// CA : CCA Threshold
// ----- Sleep Commands -----
// SM : Sleep Mode
// SO : Sleep Options
// ST : Time before Sleep
// SP : Cyclic Sleep Period
// DP : Disassociated Cyclic Sleep Period
// ----- Serial Interfacing Commands -----
bool baudrate(uint8_t baudrate)
{
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);
Serial.print("BAUDRATE: ");
String baudrate_str = XBeeATCmdsUtil::toHex(baudrate);
Serial.print(baudrate_str);
Serial.print(" ");
serial->print("ATBD");
serial->println(baudrate_str);
return dumpReply();
}
bool setPreambleID(uint8_t id)
bool packtizationTimeout(uint8_t num)
{
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);
Serial.print("PACKETIZATION TIMEOUT: ");
serial->print("ATRO");
serial->println(XBeeATCmdsUtil::toHex(num));
return dumpReply();
}
bool setNetworkID(uint16_t id)
bool apiEnable(uint8_t en)
{
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);
Serial.print("API ENABLE: ");
serial->print("ATAP");
serial->println(XBeeATCmdsUtil::toHex(en));
return dumpReply();
}
bool setBroadcastMultiTransmit(uint8_t n_times)
bool parity(uint8_t parity)
{
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);
Serial.print("PARITY: ");
serial->print("ATNB");
serial->println(XBeeATCmdsUtil::toHex(parity));
return dumpReply();
}
bool setDestinationAddr(uint32_t addr_h, uint32_t addr_l)
// PR : Pull-up/down Resistor Enable
// ----- I/O Settings Commands -----
// D0 : DIO0 Configuration
// D1 : DIO1 Configuration
// D2 : DIO2 Configuration
// D3 : DIO3 Configuration
// D4 : DIO4 Configuration
// D5 : DIO5 Configuration
// D6 : DIO6 Configuration
// D7 : DIO7 Configuration
// D8 : DIO8 Configuration
// IU : I/O Output Enable
// IT : Samples before TX
// IS : Force Sample
// IO : Digital Output Level
// IC : DIO Change Detect
// IR : Sample Rate
// IA : I/O Input Address
// T0 : D0 Output Timeout
// T1 : D1 Output Timeout
// T2 : D2 Output Timeout
// T3 : D3 Output Timeout
// T4 : D4 Output Timeout
// T5 : D5 Output Timeout
// T6 : D6 Output Timeout
// T7 : D7 Output Timeout
// P0 : PWM0 Configuration
// P1 : PWM1 Configuration
// M0 : PWM0 Output Level
// M1 : PWM1 Output Level
// PT : PWM Output Timeout
// RP : RSSI PWM Timer
// ----- Diagnostic Commands -----
bool firmwareVer() // Firmware Version
{
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());
Serial.print("FIRMWARE VERSION: ");
serial->println("ATVR");
return dumpReply();
}
bool setBaudRate(uint8_t baudrate)
bool hardwareVer() // Hardware Version
{
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);
Serial.print("HARDWARE VERSION: ");
serial->println("ATHV");
return dumpReply();
}
bool enterCommandMode()
bool rssi() // Last Packet RSSI
{
Serial.printf("ENTER COMMAND MODE: \n");
serial->printf("+++");
delay(1000);
Serial.print("LAST PACKET RSSI: ");
serial->println("ATDV");
return dumpReply();
}
bool exitCommandMode()
// EC : CCA Failures
// AC : ACK Failures
// ED : Energy Scan
// ----- Command Mode Options -----
void exit()
{
Serial.print("EXIT AT COMMAND MODE: ");
serial->println("ATCN");
}
bool apply()
{
Serial.printf("EXIT COMMAND MODE: \n");
serial->printf("ATCN\r");
Serial.print("APPLY CHANGES: ");
serial->println("ATAC");
return dumpReply();
}
// CT : Command Mode Timeout
// GT : Guard Times
// CC : Command Sequence Character
private:
bool dumpReply()
{
delay(XB_REPLY_WAIT);
if (!serial->available()) {
Serial.println("XBee NOT Responding... Timeout...");
String reply = serial->readStringUntil('\n');
if (reply == "") // timeout
{
Serial.println("XBee NOT Responding. Timeout...");
return false;
}
while(serial->available()) Serial.write(XB_SERIAL.read());
Serial.println();
else
{
Serial.print(reply);
}
return true;
}
private:
static const uint16_t WAIT_COMMAND_MODE_MS = 3000;
Stream* serial;
};
#endif // XBEEATCMDS_H
src/main.cpp
......@@ -9,13 +9,40 @@ void setup()
delay(2000);
identify();
// ----- init Eeprom ------
// ----- setup XBee -----
initEeprom(HARDWARE_ID);
delay(2000);
initXBee(HARDWARE_ID);
unpacker.attach(XBEE_SERIAL);
// ----- Emergency Switch -----
pinMode(32, INPUT_PULLUP);
// ----- check I2C -----
Wire.begin();
// i2c_Scan(Wire);
Serial.println("mpu setup");
mpu.setup(Wire);
// ----- check LED Driver -----
Serial.println("IR LED setup");
irled.setup(Wire);
pinMode(17, OUTPUT);
digitalWrite(17, LOW); // Output Enable
WHILL_SERIAL_L.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // L
WHILL_SERIAL_R.begin(WHILL_SERIAL_BAUD, SERIAL_8N2); // R
whill.attach(WHILL_SERIAL_L, WHILL_SERIAL_R);
XBEE_SERIAL.begin(XBEE_SERIAL_BAUD); // XBee
unpacker.attach(XBEE_SERIAL);
whill.streaming(false, WHILL_INFO_INTERVAL_MS, DATASET::SPEED_PROFILE);
whill.stop();
......@@ -38,6 +65,7 @@ void setup()
whill.streaming(true, WHILL_INFO_INTERVAL_MS, DATASET::SENSOR_INFO);
delay(1000);
Serial.println("start program");
}
......
  • 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