Skip to content

Hideaki Tai / omni_main

Go to a project
Commit c4396fe9 by Hideaki Tai
Options

test ok

parent 907a3c2f
Expand all
Inline Side-by-side
Showing with 511 additions and 13 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]
......
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