Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

[Sentry] Better visualization of data: show (and record) each sample …

…of encoder update
  • Loading branch information...
commit c890f07e578fd4480ffff0955ecca95ac15d62a4 1 parent 161f864
@garbear authored
View
8 avr/include/ArduinoAddressBook.h
@@ -13,10 +13,12 @@
#define FSM_MOTORCONTROLLER 9
#define FSM_SERVOSWEEP 10
#define FSM_SENTRY 11
+#define FSM_ENCODER 12
-#define MSG_MASTER_CREATE_FSM 0
-#define MSG_MASTER_DESTROY_FSM 1
-#define MSG_MASTER_LIST_FSM 2
+#define MSG_MASTER_CREATE_FSM 0
+#define MSG_MASTER_DESTROY_FSM 1
+#define MSG_MASTER_LIST_FSM 2
+#define MSG_MASTER_ENCODER_SAMPLES 3
// PWM LEDs
#define LED_GREEN 4
View
24 avr/src/MecanumMaster.cpp
@@ -103,9 +103,15 @@ void MecanumMaster::Spin()
unsigned long microsValue;
unsigned long millisValue;
unsigned long lastMicrosValue = 0;
- unsigned long lastEncoderUpdate = 0;
+ unsigned long encoderDelay = 0;
bool microsWrapped = false;
bool encoderWrapped = false;
+ uint8_t encoderSamples[19]; // 128 samples + 2 byte length + 1 byte ID (MSG_MASTER_ENCODER_SAMPLES)
+ encoderSamples[0] = 19;
+ encoderSamples[1] = 0;
+ encoderSamples[2] = MSG_MASTER_ENCODER_SAMPLES;
+ uint8_t sampleCount = 0;
+
for (;;)
{
// TODO: This needs to read length, and then only read if (length-2) is
@@ -123,10 +129,8 @@ void MecanumMaster::Spin()
}
}
- // QRE1113 rise time is 20uS, use 500us
- if (m_encoder)
+ if (m_encoder && m_encoder->IsEnabled())
{
- // Snapshot of our micros. Assume less than 55s has passed since last loop
microsValue = micros();
// microsValue may have wrapped. This is true if microsValue hasn't increased
@@ -136,18 +140,18 @@ void MecanumMaster::Spin()
microsWrapped = false;
// If a wrap occurs, swap the check. If two wraps occur, avoid the swap
- if (!(microsWrapped ^ encoderWrapped) ? (lastEncoderUpdate <= microsValue) : !(lastEncoderUpdate <= microsValue))
+ if (!(microsWrapped ^ encoderWrapped) ? (encoderDelay <= microsValue) : !(encoderDelay <= microsValue))
{
m_encoder->Update();
- // Avoid a "lastEncoderValue" by using ULONG_MAX - 40 instead of
- // another variable. We know the time delta here, unlike in the
- // "microsValue < lastMicrosValue" situation above.
- if (microsValue >= ULONG_MAX - 1000)
+
+ // Consider, QRE1113 rise time is 20uS
+ if (microsValue >= ULONG_MAX - 500)
encoderWrapped = true;
else if (encoderWrapped)
encoderWrapped = false;
- lastEncoderUpdate = microsValue + 1000;
+ encoderDelay = microsValue + 500;
}
+ lastMicrosValue = microsValue;
}
}
}
View
53 avr/src/Sentry.cpp
@@ -35,18 +35,18 @@
#define NOMINAL_uS_PER_TICK 30 // (1000 us / 24 degrees) * (360 degrees / TICKS)
-void Encoder::Start()
+Encoder::Encoder(uint8_t pin) : m_pin(pin), m_ticks(0), m_state(0), m_sampleCount(0), m_enabled(false)
{
pinMode(m_pin, INPUT);
pinMode(LED_BATTERY_EMPTY, OUTPUT);
- m_ticks = 0;
- m_state = digitalRead(m_pin);
}
-void Encoder::Reset()
+void Encoder::Start()
{
m_ticks = 0;
m_state = digitalRead(m_pin);
+ m_sampleCount = 0; // Redundant
+ m_enabled = true;
}
void Encoder::Update()
@@ -57,13 +57,45 @@ void Encoder::Update()
m_ticks++;
digitalWrite(LED_BATTERY_EMPTY, m_state);
}
+ // Clear the byte if newly accessed
+ if (m_sampleCount % 8 == 0)
+ m_sampleMessage[m_sampleCount / 8 + 4] = 0;
+
+ m_sampleMessage[m_sampleCount / 8 + 4] |= m_state << (m_sampleCount % 8);
+
+ if (++m_sampleCount == 8 * (sizeof(m_sampleMessage) - 4))
+ Publish();
+}
+
+void Encoder::Disable()
+{
+ if (m_enabled)
+ {
+ m_enabled = false;
+ Publish();
+ }
+}
+
+void Encoder::Publish()
+{
+ // Make sure we actually have data to publish
+ if (m_sampleCount == 0)
+ return;
+
+ // Only publish what's needed (1 extra byte for every 9th bit)
+ m_sampleMessage[0] = 5 + (m_sampleCount - 1) / 8;
+ m_sampleMessage[1] = 0;
+ m_sampleMessage[2] = FSM_ENCODER;
+ m_sampleMessage[3] = m_sampleCount;
+ Serial.write(m_sampleMessage, 5 + (m_sampleCount - 1) / 8);
+
+ // Reset the samples array
+ m_sampleCount = 0;
}
Sentry::Sentry() : m_encoder(ENCODER_PIN), m_state(SEEKING_MIDPOINT), m_targetMicros(INITIAL_MIDPOINT)
{
Init(FSM_SENTRY, m_params.GetBuffer());
-
- m_encoder.Start();
m_servo.attach(SERVO_PIN);
}
@@ -93,16 +125,19 @@ uint32_t Sentry::Step()
{
// Assume we already arrived at the midpoint
if (m_targetMicros == INITIAL_MIDPOINT)
- m_encoder.Reset();
+ {
+ m_encoder.Start();
+ }
if (m_targetMicros - NOMINAL_uS_PER_TICK < 800 || m_targetMicros + NOMINAL_uS_PER_TICK > 2200)
{
// Publish the number of ticks it took to get here
ParamServer::SentryPublisherMsg msg;
msg.SetTicks(m_encoder.Ticks());
+ m_encoder.Disable();
// Center around INITIAL_MIDPOINT
msg.SetMicroseconds(m_targetMicros - INITIAL_MIDPOINT);
- Serial.write(msg.GetBytes(), msg.GetLength());
+ //Serial.write(msg.GetBytes(), msg.GetLength());
m_targetMicros = INITIAL_MIDPOINT;
m_servo.writeMicroseconds(m_targetMicros);
@@ -116,7 +151,7 @@ uint32_t Sentry::Step()
m_targetMicros += NOMINAL_uS_PER_TICK;
m_servo.writeMicroseconds(m_targetMicros);
- return 100;
+ return 50;
}
case FINISHED:
default:
View
15 avr/src/Sentry.h
@@ -26,21 +26,32 @@
#include <Servo.h>
+/**
+ * Track a shaft encoder with an IR sensor.
+ */
class Encoder
{
public:
- Encoder(uint8_t pin) : m_pin(pin), m_ticks(0), m_state(0) { }
+ Encoder(uint8_t pin);
void Start();
void Update();
int Ticks() const { return m_ticks; }
- void Reset();
+
+ void Disable();
+ bool IsEnabled() const { return m_enabled; }
private:
+ void Publish();
+
uint8_t m_pin;
int m_ticks;
uint8_t m_state;
+ // Use <= 128 samples, otherwise we overflow
+ uint8_t m_sampleMessage[16 + 4]; // 128 samples + 4 byte header
+ uint8_t m_sampleCount;
+ bool m_enabled;
};
View
99 avr_controller/src/SentryMonitor.cpp
@@ -29,6 +29,8 @@
#include <unistd.h> // for usleep()
#include <string>
#include <iostream>
+#include <fstream>
+#include <time.h>
using namespace std;
@@ -57,30 +59,87 @@ void SentryMonitor::Main()
ParamServer::Sentry sentry;
m_arduino.CreateFSM(sentry.GetString());
- string response;
- if (m_arduino.Receive(FSM_SENTRY, response, 5000)) // 3s
+ int timeout = 0;
+ string samples;
+ while (timeout <= 5000)
{
- ParamServer::SentryPublisherMsg msg(response);
- cout << "Left: Ticks: " << (int)msg.GetTicks() << ", Microseconds: " << msg.GetMicroseconds() << endl;
- cout << "Left: uS/tick: " << (1.0f * msg.GetMicroseconds() / msg.GetTicks() * 360 / 100) << endl;
- // 1000 us / 24 degrees * 360 degrees / 100 ticks = 30 uS/tick
- cout << "Left: estimated uS/tick: -150 uS/tick" << endl;
- cout << "Left: uS/degree: " << (1.0f * msg.GetMicroseconds() / msg.GetTicks()) << endl;
- // 1000 us / 24 degrees = 8.33 uS/degree
- cout << "Left: estimated uS/degree: -41.66 uS/degree" << endl;
- cout << endl;
-
- if (m_arduino.Receive(FSM_SENTRY, response, 5000)) // 3s
+ string response;
+ // Publish period = 1ms / 2 samples * 128 samples / message = 64ms
+ if (m_arduino.Receive(FSM_ENCODER, response, 128))
{
- ParamServer::SentryPublisherMsg msg2(response);
- cout << "Right: Ticks: " << (int)msg2.GetTicks() << ", Microseconds: " << msg2.GetMicroseconds() << endl;
- cout << "Right: uS/tick: " << (1.0f * msg2.GetMicroseconds() / msg2.GetTicks() * 360 / 100) << endl;
- cout << "Right: uS/degree: " << (1.0f * msg2.GetMicroseconds() / msg2.GetTicks()) << endl;
+ // Parse the response manually
+ if (response.length() <= 4)
+ {
+ cout << "Error: Invalid message length (" << response.length() << ")" << endl;
+ break; // Invalid message
+ }
- gpio.SetValue(0);
- return;
+ const uint8_t *bytes = reinterpret_cast<const uint8_t*>(response.c_str());
+ unsigned int sampleCount = bytes[3];
+ // Make sure we don't overshoot the size of the array
+ if (sampleCount > 8 * (response.length() - 4))
+ {
+ cout << "Error: Invalid samples size (" << sampleCount << ")" << endl;
+ break;
+ }
+
+ bytes += 4; // Fast-forward to pertinent data
+
+ for (unsigned int i = 0; i < sampleCount; i++)
+ {
+ samples.push_back(bytes[i / 8] & (1 << (i % 8)) ? '1' : '0');
+ samples.push_back(' ');
+ }
+
+ timeout = 0;
+ }
+ else
+ {
+ if (samples.length())
+ {
+ // Finished gathering our set of samples
+ Process(samples);
+ samples = "";
+ }
+ timeout += 128;
+ // Report every second
+ if (timeout % 1000 < 128)
+ cout << "Timeout (" << timeout << " ms)..." << endl;
}
}
- cout << "Error: Failed to receive response" << endl;
+ cout << "Finished receiving data" << endl;
+ m_arduino.DestroyFSM(sentry.GetString());
gpio.SetValue(0);
}
+
+void SentryMonitor::Process(const string &samples)
+{
+ static char i = '0';
+
+ //get the current time from the clock -- one second resolution
+ string filename = CurrentDateTime() + "-" + i + ".txt";
+ ofstream myfile;
+ myfile.open(filename.c_str());
+ myfile << samples;
+ myfile.close();
+
+ if (i++ == '9')
+ i = '0';
+
+ cout << "Samples:" << endl;
+ cout << samples << endl << endl;
+}
+
+// Get current date/time, format is YYYY-MM-DD.HH:mm:ss
+const std::string SentryMonitor::CurrentDateTime()
+{
+ time_t now = time(0);
+ struct tm tstruct;
+ char buf[80];
+ tstruct = *localtime(&now);
+ // Visit http://www.cplusplus.com/reference/clibrary/ctime/strftime/
+ // for more information about date/time format
+ strftime(buf, sizeof(buf), "%Y-%m-%d.%X", &tstruct);
+ return buf;
+}
+
View
3  avr_controller/src/SentryMonitor.h
@@ -30,7 +30,10 @@ class SentryMonitor
SentryMonitor() { }
void Main();
+ static const std::string CurrentDateTime();
+
private:
+ void Process(const std::string &samples);
AVRController m_arduino;
};
Please sign in to comment.
Something went wrong with that request. Please try again.