Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

rewrite of arduino firmware

  • Loading branch information...
commit 1e477a5fa834c9f8e04ab5fe45e32fc1c6f6c8a2 1 parent a72ed43
@sagargp sagargp authored
View
74 Robotics/Drivers/Hermes/Firmware/hermes/ByteBuffer.h
@@ -0,0 +1,74 @@
+/*
+ ByteBuffer.h - A circular buffer implementation for Arduino
+ Created by Sigurdur Orn, July 19, 2010.
+ siggi@mit.edu
+ */
+
+#ifndef ByteBuffer_h
+#define ByteBuffer_h
+
+#include "Arduino.h"
+
+class ByteBuffer
+{
+public:
+ ByteBuffer();
+
+ // This method initializes the datastore of the buffer to a certain sizem the buffer should NOT be used before this call is made
+ void init(unsigned int buf_size);
+
+ // This method resets the buffer into an original state (with no data)
+ void clear();
+
+ // This releases resources for this buffer, after this has been called the buffer should NOT be used
+ void deAllocate();
+
+ // Returns how much space is left in the buffer for more data
+ int getSize();
+
+ // Returns the maximum capacity of the buffer
+ int getCapacity();
+
+ // This method returns the byte that is located at index in the buffer but doesn't modify the buffer like the get methods (doesn't remove the retured byte from the buffer)
+ byte peek(unsigned int index);
+
+ //
+ // Put methods, either a regular put in back or put in front
+ //
+ int putInFront(byte in);
+ int put(byte in);
+
+ int putIntInFront(int in);
+ int putInt(int in);
+
+ int putLongInFront(long in);
+ int putLong(long in);
+
+ int putFloatInFront(float in);
+ int putFloat(float in);
+
+ //
+ // Get methods, either a regular get from front or from back
+ //
+ byte get();
+ byte getFromBack();
+
+ int getInt();
+ int getIntFromBack();
+
+ long getLong();
+ long getLongFromBack();
+
+ float getFloat();
+ float getFloatFromBack();
+
+private:
+ byte* data;
+
+ unsigned int capacity;
+ unsigned int position;
+ unsigned int length;
+};
+
+#endif
+
View
212 Robotics/Drivers/Hermes/Firmware/hermes/ByteBuffer.ino
@@ -0,0 +1,212 @@
+/*
+ ByteBuffer.cpp - A circular buffer implementation for Arduino
+ Created by Sigurdur Orn, July 19, 2010.
+ siggi@mit.edu
+ */
+
+#include <util/atomic.h>
+#include "Arduino.h"
+#include "ByteBuffer.h"
+
+
+ByteBuffer::ByteBuffer(){
+
+}
+
+void ByteBuffer::init(unsigned int buf_length){
+ data = (byte*)malloc(sizeof(byte)*buf_length);
+ capacity = buf_length;
+ position = 0;
+ length = 0;
+}
+
+void ByteBuffer::deAllocate(){
+ free(data);
+}
+
+void ByteBuffer::clear(){
+ position = 0;
+ length = 0;
+}
+
+int ByteBuffer::getSize(){
+ return length;
+}
+
+int ByteBuffer::getCapacity(){
+ return capacity;
+}
+
+byte ByteBuffer::peek(unsigned int index){
+ byte b = data[(position+index)%capacity];
+ return b;
+}
+
+int ByteBuffer::put(byte in){
+ if(length < capacity){
+ // save data byte at end of buffer
+ data[(position+length) % capacity] = in;
+ // increment the length
+ length++;
+ return 1;
+ }
+ // return failure
+ return 0;
+}
+
+int ByteBuffer::putInFront(byte in){
+ if(length < capacity){
+ // save data byte at end of buffer
+ if( position == 0 )
+ position = capacity-1;
+ else
+ position = (position-1)%capacity;
+ data[position] = in;
+ // increment the length
+ length++;
+ return 1;
+ }
+ // return failure
+ return 0;
+}
+
+byte ByteBuffer::get(){
+ byte b = 0;
+
+
+ if(length > 0){
+ b = data[position];
+ // move index down and decrement length
+ position = (position+1)%capacity;
+ length--;
+ }
+
+ return b;
+}
+
+byte ByteBuffer::getFromBack(){
+ byte b = 0;
+ if(length > 0){
+ b = data[(position+length-1)%capacity];
+ length--;
+ }
+
+ return b;
+}
+
+//
+// Ints
+//
+
+int ByteBuffer::putIntInFront(int in){
+ byte *pointer = (byte *)&in;
+ putInFront(pointer[0]);
+ putInFront(pointer[1]);
+}
+
+int ByteBuffer::putInt(int in){
+ byte *pointer = (byte *)&in;
+ put(pointer[1]);
+ put(pointer[0]);
+}
+
+
+int ByteBuffer::getInt(){
+ int ret;
+ byte *pointer = (byte *)&ret;
+ pointer[1] = get();
+ pointer[0] = get();
+ return ret;
+}
+
+int ByteBuffer::getIntFromBack(){
+ int ret;
+ byte *pointer = (byte *)&ret;
+ pointer[0] = getFromBack();
+ pointer[1] = getFromBack();
+ return ret;
+}
+
+//
+// Longs
+//
+
+int ByteBuffer::putLongInFront(long in){
+ byte *pointer = (byte *)&in;
+ putInFront(pointer[0]);
+ putInFront(pointer[1]);
+ putInFront(pointer[2]);
+ putInFront(pointer[3]);
+}
+
+int ByteBuffer::putLong(long in){
+ byte *pointer = (byte *)&in;
+ put(pointer[3]);
+ put(pointer[2]);
+ put(pointer[1]);
+ put(pointer[0]);
+}
+
+
+long ByteBuffer::getLong(){
+ long ret;
+ byte *pointer = (byte *)&ret;
+ pointer[3] = get();
+ pointer[2] = get();
+ pointer[1] = get();
+ pointer[0] = get();
+ return ret;
+}
+
+long ByteBuffer::getLongFromBack(){
+ long ret;
+ byte *pointer = (byte *)&ret;
+ pointer[0] = getFromBack();
+ pointer[1] = getFromBack();
+ pointer[2] = getFromBack();
+ pointer[3] = getFromBack();
+ return ret;
+}
+
+
+//
+// Floats
+//
+
+int ByteBuffer::putFloatInFront(float in){
+ byte *pointer = (byte *)&in;
+ putInFront(pointer[0]);
+ putInFront(pointer[1]);
+ putInFront(pointer[2]);
+ putInFront(pointer[3]);
+}
+
+int ByteBuffer::putFloat(float in){
+ byte *pointer = (byte *)&in;
+ put(pointer[3]);
+ put(pointer[2]);
+ put(pointer[1]);
+ put(pointer[0]);
+}
+
+float ByteBuffer::getFloat(){
+ float ret;
+ byte *pointer = (byte *)&ret;
+ pointer[3] = get();
+ pointer[2] = get();
+ pointer[1] = get();
+ pointer[0] = get();
+ return ret;
+}
+
+float ByteBuffer::getFloatFromBack(){
+ float ret;
+ byte *pointer = (byte *)&ret;
+ pointer[0] = getFromBack();
+ pointer[1] = getFromBack();
+ pointer[2] = getFromBack();
+ pointer[3] = getFromBack();
+ return ret;
+}
+
+
View
37 Robotics/Drivers/Hermes/Firmware/hermes/globals.h
@@ -1,37 +0,0 @@
-#ifndef GLOBALS_H
-#define GLOBALS_H
-
-//
-// Debug
-//
-// #define DEBUG
-
-#define POS __FUNCTION__ + "() line " + __LINE__ + ":\t"
-
-#ifdef DEBUG
-#define LOG_RAW(message) Serial.write(message);
-#define LOG(message) { String msg = ""; Serial.print(msg + POS); Serial.println(message); }
-#else
-#define LOG_RAW(message) ;
-#define LOG(message) ;
-#endif
-
-#ifndef DEBUG
-#define SERIALIZE(type, object, var) { byte checksum = 0; Serial.write(type); for(int i=0; i<sizeof(object); i++) { Serial.write(var.raw[i]); checksum ^= var.raw[i]; } Serial.write(checksum); }
-#else
-#define SERIALIZE(type, object, var) Serial.print("Sensor Data Type: "); Serial.print(type); Serial.print(" Payload: "); for(int i=0; i<sizeof(object); i++) { Serial.print(var.raw[i]); Serial.print(" "); } Serial.println("");
-#endif
-
-#ifndef DEBUG
-#define RATE_LIMIT(duration) if(millis() % duration == 0)
-#else
-#define RATE_LIMIT(duration) if(millis() % 2000 == 0)
-#endif
-
-#ifndef DEBUG
-#define MAX_SERIAL_DELAY 100
-#else
-#define MAX_SERIAL_DELAY 500
-#endif
-
-#endif
View
105 Robotics/Drivers/Hermes/Firmware/hermes/hermes.h
@@ -1,15 +1,100 @@
-#ifndef HERMES_H
-#define HERMES_H
+#ifndef _HERMES_H
+#define _HERMES_H
-// Pins
-#define BATTERY_IN A0
-#define DIGITAL_RELAY 52
-#define LEFT_SERVO 9
-#define RIGHT_SERVO 10
+#define MOTOR_PWM_OFFSET 6
+#define BATTERY_ADJUSTMENT 0.0214
+#define MAGNETIC_DECLINATION 219/1000.0 // declination in LA
-#define BAUDRATE 9600
+#define BAUDRATE 9600
+#define WATCHDOG_THRESHOLD 1000
-#define WATCHDOG_THRESHOLD 1000
+// pin defs
+#define LEFT_SERVO_PIN 9
+#define RIGHT_SERVO_PIN 10
+#define BATTERY_PIN A0
+#define DIGITAL_RELAY 52
+// packet definitions
+union MotorPacket
+{
+ byte raw[2];
+ struct
+ {
+ byte left;
+ byte right;
+ };
+};
-#endif
+union BatteryPacket
+{
+ byte raw[4];
+ float voltage;
+};
+
+union CompassPacket
+{
+ byte raw[4];
+ float heading;
+};
+
+union GyroPacket
+{
+ byte raw[12];
+ float xyz[3];
+ struct
+ {
+ float x;
+ float y;
+ float z;
+ };
+};
+
+// helper function for packets and packet ID definitions
+template<class PacketT>
+byte packet_id();
+
+template<> byte packet_id<CompassPacket>() { return 99; }
+template<> byte packet_id<GyroPacket>() { return 100; }
+template<> byte packet_id<BatteryPacket>() { return 101; }
+
+// bookkeeping vars
+ByteBuffer buffer;
+unsigned int watchdog;
+
+// motors
+Servo leftMotor;
+Servo rightMotor;
+
+// sensors
+HMC5883L magnetometer;
+MagnetometerRaw magnetometerRaw;
+ITG3200 gyro;
+MedianFilter *battery;
+
+template<class PacketT>
+void sendPacket(PacketT const& packet)
+{
+ byte packetId = packet_id<PacketT>();
+
+ Serial.write(255);
+ Serial.write(packetId);
+
+ byte checksum = 255 ^ packetId;
+ for (int i = 0; i < sizeof(PacketT); i++)
+ {
+ checksum ^= packet.raw[i];
+ Serial.write(packet.raw[i]);
+ }
+ Serial.write(checksum);
+}
+
+void setMotors(int left, int right)
+{
+ if (left < 0 || right < 0)
+ return;
+
+ leftMotor.writeMicroseconds(map(left, 0, 128, 1000, 2000)-MOTOR_PWM_OFFSET);
+ rightMotor.writeMicroseconds(map(right, 0, 128, 1000, 2000)-MOTOR_PWM_OFFSET);
+}
+
+#endif // _HERMES_H
View
292 Robotics/Drivers/Hermes/Firmware/hermes/hermes.ino 100755 → 100644
@@ -1,249 +1,95 @@
-#include "hermes.h"
#include <Wire.h>
#include <Servo.h>
-// #include <ADXL345.h>
+#include "Arduino.h"
#include "HMC5883L.h"
#include "ITG3200.h"
-#include "globals.h"
-#include "serialdata.h"
#include "MedianFilter.h"
+#include "ByteBuffer.h"
+#include "hermes.h"
-class Motors
+void setup()
{
-public:
- Servo Left;
- Servo Right;
- bool enabled;
-
-public:
- Motors ()
- {
- // Attach servos
- Left.attach(LEFT_SERVO);
- Right.attach(RIGHT_SERVO);
-
- enabled = false;
-
- setSpeed(64,64);
- }
-
- /**
- * Scales speed (0-128) to motor speed (1000-2000). Idle is 64 and 1500, respectively.
- */
- unsigned int microsFromByte(byte speed)
- {
- return map(speed, 0, 128, 1000, 2000);
- }
-
- void setSpeed(int leftSpeed, int rightSpeed)
- {
- if(!enabled)
- {
- leftSpeed = 64;
- rightSpeed = 64;
- }
-
- if(leftSpeed < 0 || rightSpeed < 0)
- return;
-
- int writeLeft = (leftSpeed == 64) ? 0 : microsFromByte(leftSpeed);
- int writeRight = (rightSpeed == 64) ? 0 : microsFromByte(rightSpeed);
- LOG(leftSpeed);
- Left.writeMicroseconds(microsFromByte(leftSpeed-6)); // 6 is an offset for these defective motor drivers
- Right.writeMicroseconds(microsFromByte(rightSpeed-6));
- }
-
- void disable()
- {
- enabled = false;
- }
-
- void enable()
- {
- enabled = true;
- }
-};
+ Serial.begin(9600);
+ leftMotor.attach(LEFT_SERVO_PIN);
+ rightMotor.attach(RIGHT_SERVO_PIN);
-class Sensors
-{
-public:
- HMC5883L magnetometer;
- ITG3200 gyro;
+ buffer.init(4);
- MagnetometerRaw mag;
- compassPacket magPacket;
+ magnetometer.SetMeasurementMode(Measurement_Continuous);
- MedianFilter *battery;
-
-public:
- Sensors ()
- {
- // IMU
- // magnetometer = HMC5883L();
- magnetometer.SetMeasurementMode(Measurement_Continuous);
-
- // gyro = ITG3200();
- gyro.reset();
- gyro.init(ITG3200_ADDR_AD0_LOW);
- // // gyro.zeroCalibrate(2500,2); // samples,seconds
- //
- battery = new MedianFilter(1024);
- }
-
- ~Sensors() { delete battery; }
-
- void tick()
+ gyro.reset();
+ gyro.init(ITG3200_ADDR_AD0_LOW);
+
+ battery = new MedianFilter(1024);
+
+ watchdog = millis();
+}
+
+void loop()
+{
+ // read a byte
+ if (Serial.available() > 0)
+ buffer.put(Serial.read());
+
+ // check the buffer for motor commands
+ if (buffer.peek(0) == 255)
{
- RATE_LIMIT(200) {
- battery->push(analogRead(BATTERY_IN));
- batteryPacket bp;
- bp.voltage = 0.0214*battery->median();
-
- // if(bp.voltage < 13){
- // digitalWrite(DIGITAL_RELAY, LOW);
- // }
+ byte checksum = 0;
+ for (int i = 0; i < sizeof(MotorPacket)+2; i++)
+ checksum ^= buffer.peek(i);
+
+ if (checksum == buffer.peek(sizeof(MotorPacket)+1))
+ {
+ MotorPacket packet;
- SERIALIZE(SEN_BATTERY, batteryPacket, bp);
- }
-
- RATE_LIMIT(200){
- mag = magnetometer.ReadRawAxis();
- magPacket.heading = atan2(mag.YAxis, mag.XAxis);
- float declination = 219/1000.0; // Magnetic declination in Los Angeles
- magPacket.heading += declination;
+ // discard the start byte
+ buffer.get();
+ for (int i = 0; i < sizeof(MotorPacket); i++)
+ packet.raw[i] = buffer.get();
+
+ // discard the checksum byte
+ buffer.get();
- // Normalize
- if(magPacket.heading < 0)
- magPacket.heading += 2*PI;
- else if(magPacket.heading > 2*PI)
- magPacket.heading -= 2*PI;
-
- SERIALIZE(SEN_COMPASS, compassPacket, magPacket);
- }
-
- return;
-
- // Gyro
- RATE_LIMIT(100){
- if(gyro.isRawDataReady())
- {
- gyroPacket packet;
- // gyro.readGyro(packet.xyz);
- //
- // SERIALIZE(SEN_GYRO, gyroPacket, packet);
- }
+ // set the motors
+ setMotors(packet.left, packet.right);
+
+ // reset watchdog
+ watchdog = millis();
}
}
-};
-typedef enum {IDLE, ACTIVE} State;
-class Hermes
-{
-public:
- Motors motors;
- Sensors sensors;
- State state;
- unsigned int lastUpdate;
-
-public:
- Hermes ()
+ // write any sensor data
{
- setState(IDLE);
- lastUpdate = millis();
+ battery->push(analogRead(BATTERY_PIN));
+ BatteryPacket bp;
+ bp.voltage = BATTERY_ADJUSTMENT*battery->median();
+ sendPacket(bp);
}
-
- void setState(State newState)
- {
- if(newState == IDLE)
- {
- motors.setSpeed(64,64);
- motors.disable();
- }
- if(newState == ACTIVE)
- {
- motors.enable();
- }
- state = newState;
- }
-
- void dispatch(char cmd)
+
{
- switch(cmd)
- {
- case(CMD_RESET):
- motors.enable();
- setState(ACTIVE);
- break;
-
- case(CMD_SETSPEED):
- {
- // Deserialize
- motorSpeedPacket msp;
- bool failed = false;
- for(int i=0; i<sizeof(motorSpeedPacket); i++){
- if(Serial.available() > 0)
- msp.raw[i] = Serial.read();
- else
- {
- int start = millis();
- bool didRead = false;
- while(start < (millis() + MAX_SERIAL_DELAY)) {
- if(Serial.available() > 0){
- msp.raw[i] = Serial.read();
- didRead = true;
- break;
- }
- }
- if(!didRead) failed = true;
- }
- }
- if(failed) {
- msp.values.left = 64;
- msp.values.right = 64;
- }
-
- motors.setSpeed(msp.values.left, msp.values.right);
- } break; // case(CMD_SETSPEED)
-
- default:
- LOG("Unrecognized cmd");
- break;
- }
+ CompassPacket cp;
+ MagnetometerRaw raw = magnetometer.ReadRawAxis();
+ cp.heading = atan2(raw.YAxis, raw.XAxis);
+ cp.heading += MAGNETIC_DECLINATION;
+
+ // normalize
+ if (cp.heading < 0)
+ cp.heading += 2*PI;
+ else if (cp.heading > 2*PI)
+ cp.heading -= 2*PI;
+
+ sendPacket(cp);
}
-
- void tick()
+
{
- if(state == ACTIVE)
+ if (gyro.isRawDataReady())
{
- if(millis()%65536 > (lastUpdate + WATCHDOG_THRESHOLD)){
- setState(IDLE);
- }
+ GyroPacket gp;
+ gyro.readGyro(gp.xyz);
+ sendPacket(gp);
}
-
- if (Serial.available() > 0) {
- dispatch(Serial.read());
- lastUpdate = millis()%65536;
- } else {
-
- }
-
- sensors.tick();
}
-};
-
-static Hermes* hermes;
-void setup()
-{
- Serial.begin(BAUDRATE);
- Wire.begin();
- hermes = new Hermes();
-
- // Power computer
- digitalWrite(DIGITAL_RELAY, HIGH);
-}
-
-void loop()
-{
- hermes->tick();
+ if (millis() - watchdog > WATCHDOG_THRESHOLD)
+ setMotors(64, 64);
}
View
34 Robotics/Drivers/Hermes/Firmware/hermes/serialdata.h
@@ -1,34 +0,0 @@
-#ifndef SERIALDATA_H
-#define SERIALDATA_H
-
-#define SEN_COMPASS 99
-union compassPacket {
- unsigned char raw[4];
- float heading;
-};
-
-#define SEN_GYRO 100
-union gyroPacket {
- unsigned char raw[12];
- float xyz[3];
-};
-
-#define SEN_BATTERY 101
-union batteryPacket {
- unsigned char raw[4];
- float voltage;
-};
-
-// Commands
-#define CMD_RESET 97
-#define CMD_SETSPEED 98 // {leftSpeed 0-127} {rightSpeed 0-127}
-
-union motorSpeedPacket { // TODO
- unsigned char raw[2];
- struct {
- unsigned char left;
- unsigned char right;
- } values;
-};
-
-#endif // SERIALDATA_H
Please sign in to comment.
Something went wrong with that request. Please try again.