brmlab

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
<code>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>


/* 1:1 with EEPROM contents */
struct config {
  byte is_initialized[3]; // 'BRM'

  /* Length of periods when motor is on/off; in 10ms units. */
  /* Well, why duration_{on,off} is actually NOT an array with separate config for each motor?
   During some experiments on our volunteers (well, actually it was an experiment on me),
   I found out, an user's brain get used very quickly (in about a minute) on a periodic
   signal and when you change the period (even a bit - could be 20 ms or around 3 %),
   the feeling is really weird.
   Of course it's cool to test it in a laboratory (I suggest testing with ECG attached;
   I bet there would be at least 5/min rise), where you control the anklet from a computer.
   But it's absolutely impossible to go with it in the real life. Yes, you could be
   (and probably are) more psychically resistent than me, but I think it would be
   strange on everybody.
   And, of course, we save 28 B of our little memory ;-)
   */
  /* 3 */ byte duration_off;
  /* 4 */ byte duration_on;

  /* It's good time to mentoin physical construction.
   Motors are counted from 0 to 7 or from North (N) to NorthWest (NW) clockwise.

   And about the intensity.
   0 should be off (to conserver power).
   1 should be the lowest perceptable boundary.
   255 should be the boundary where the motors start burning.
   But currently this is broken, we have to do some measurements on volunteers
   (again me…) and real velcro paw and I don't have required hardware at home as of
   I'm writing this code.
   Finally, there should be a curve (and it could be non-linear) stored in EEPROM
   describing this.
   */
  /* 5 */ byte pwm_min; // PWM duty cycle for intensity 1
  /* 6 */ byte pwm_max; // PWM duty cycle for intensity 255

  /* Bearing with 2\deg precision. It denotes which angle relative to current
   * heading will the motors "point" you at. I.e. bearing==0 will mean that
   * the motor currently to the north will keep vibrating, while bearing==180/2
   * will mean that the south motor will keep vibrating. */
  /* 7 */ byte bearing;

  /* After fadeout_start*10 seconds, start linearly reducing intensity; after
   * another fadeout_length*10 seconds, stop vibrating altogether. */
  /* 8 */ byte fadeout_start;
  /* 9 */ byte fadeout_length;
  /* Angle changes under max_jitter are not considered direction changes for gradual
   * vibration fade-out. */
  /* 10 */ byte max_jitter;

  /* 11-18 */ byte motor_intensity[8]; // "relative" PWM duty cycle

  /* 19-26 */ byte motor_angle_start[8]; // 2\deg precision
};

struct config config;
const PROGMEM struct config default_config = {
  /*.is_initialized =*/ { 'B', 'R', 'M' },
  /*.duration_off =*/ 500/10,
  /*.duration_on =*/ 100/10,
  /*.pwm_min =*/ 30,
  /*.pwm_max =*/ 160,
  /*.bearing =*/ 0,
  /*.fadeout_start =*/ 300/10,
  /*.fadeout_length =*/ 60/10,
  /*.max_jitter =*/ 10,
  /*.motor_intensity =*/ { 150, 150, 150, 150, 150, 150, 150, 150 },
  // 45\deg section for each motor; motor 0 is around angle 0
  /*.motor_angle_start =*/ { 174, 11, 34, 56, 79, 101, 124, 146 },
};


const byte ShiftEnable = 4;             // Every time you use variable instead of constant and int instead of byte, God kills a kitten and wastes 2 B of memory.
const byte ShiftSerIn = 2;
const byte ShiftClear = 3;
const byte ShiftClock = 5;
const byte ShiftCommit = 6;

byte heading = 0;
unsigned long CurrTime = 0;            // Hmm, my first 4 B of memory I've allocated out of that 1 KiB.
unsigned long NextEnvelopeSwitch = 0;
boolean EnvelopeStatus = false;        // Starting with motor down.
byte CurrMotor = 0;
boolean ExternalControl = false;       // Let's assume this is primarily a compass.
byte pwm;

byte ref_heading = 0;
unsigned long ref_heading_time = 0;

boolean KeyboardControl = false;

byte packet = 0;  // Packet to send to the shift register.
byte ManualIntensity = 60;
byte FadeoutIntensity = 255;


void config_defaults(void) {
  memcpy_P(&config, &default_config, sizeof(config));
}

void config_load(void) {
  eeprom_read_block(&config, (void*) 0, sizeof(config));
  if (memcmp(config.is_initialized, "BRM", 3)) {
    config_defaults();
  }
}

void config_save(void) {
  eeprom_write_block(&config, (void*) 0, sizeof(config));
}

byte *config_as_buf(struct config *c)
{
  union config_buf {
    struct config c;
    byte b[sizeof(struct config)];
  };
  return ((union config_buf*) c)->b;
}


void setup() {
  pinMode(ShiftEnable, OUTPUT);
  pinMode(ShiftSerIn, OUTPUT);
  pinMode(ShiftClear, OUTPUT);
  pinMode(ShiftClock, OUTPUT);
  pinMode(ShiftCommit, OUTPUT);

  config_load();

  digitalWrite(ShiftEnable, HIGH);      // disable shift register output
  digitalWrite(ShiftClear, LOW);        // zero the buffer
  digitalWrite(ShiftCommit, LOW);
  digitalWrite(ShiftClock, LOW);        // the TPIC6 clocks work on a rising edge, so make sure they're low to start.
  digitalWrite(ShiftClear, HIGH);       // stop clearing the buffer

  Serial.begin(9600);
  Serial.println("Console ready, send command or ? for help.");
}

void loop() {
  // TODO ReadCompass
  CurrTime = millis();
  FadeIntensity();
  ChangeMotor();
  ManageEnvelope();
  ServeConsole();
}


void FadeIntensity() {
  FadeoutIntensity = 255;
  if (abs(ref_heading - heading) > config.max_jitter) {
    ref_heading = heading;
    ref_heading_time = CurrTime;
    return;
  }
  unsigned long fadeout_start = ((unsigned long) config.fadeout_start) * (1000 * 10);
  unsigned long fadeout_length = ((unsigned long) config.fadeout_length) * (1000 * 10);
  long fadeout_time_ofs = (CurrTime - ref_heading_time) - fadeout_start;
  if (fadeout_time_ofs < 0)
    return;
  if (fadeout_time_ofs > fadeout_length) {
    FadeoutIntensity = 0;
    return;
  }
  FadeoutIntensity = ((unsigned long) config.motor_intensity[CurrMotor]) * (fadeout_length - fadeout_time_ofs) / fadeout_length;
  // Serial.println(FadeoutIntensity, DEC);
}

void ManageEnvelope() {
  /* Yo we herd you like rectangle wawes, so we put rectangle wave into a rectangle envelope,
   so u can generate rectangle wave while u are generating rectangle wave! */

  if (NextEnvelopeSwitch <= CurrTime) {
    // Yea, we have to switch the state!
    if (EnvelopeStatus) {
      // Motor is ON, so we will turn it OFF
      NextEnvelopeSwitch = CurrTime + config.duration_off*10;
    } 
    else {
      // Motor is OFF, so we will turn it ON, what's the problem?! (--Moss, The IT Crowd)
      NextEnvelopeSwitch = CurrTime + config.duration_on*10;
    }
    EnvelopeStatus = !EnvelopeStatus;
    if (EnvelopeStatus) {
      if(!ExternalControl) {
	byte intensity = config.motor_intensity[CurrMotor];
	if (FadeoutIntensity < intensity) intensity = FadeoutIntensity;
        pwm = ComputePWM(intensity);
      } 
      else {
        pwm = ComputePWM(ManualIntensity);
      }
      //Serial.println(255 - pwm, DEC);
      analogWrite(ShiftEnable, 255 - pwm);
    } 
    else {
      analogWrite(ShiftEnable, 255);    // Yes, really, 255 is off. Some documentation is going to be written about this - maybe.
    }
  }
}

void ChangeMotor() {
  if(!ExternalControl) {
    // Select motor
    byte i=0;
    CurrMotor = 0;
    byte motor_angle = (config.bearing + heading) % (360/2);
    for (i=1; i<8; i++) {
      if (config.motor_angle_start[i] > motor_angle) {
        CurrMotor = i - 1;
	break;
      }
    }
    // Compute binary code
    packet = 0;
    bitWrite(packet, CurrMotor, 1);
  }
  // Pipe it into buffer
  shiftOut(ShiftSerIn, ShiftClock, LSBFIRST, packet);
  // And commit
  delayMicroseconds(100);
  digitalWrite(ShiftCommit, HIGH);
  delayMicroseconds(100);
  digitalWrite(ShiftCommit, LOW);  // 10 kHz ought to be enough…
}

byte ComputePWM(byte Intensity) {
  byte DutyCycle;
  DutyCycle = Intensity ? map(Intensity, 1, 255, config.pwm_min, config.pwm_max) : 0;
  return DutyCycle;                             // Duh.
}

void ServeConsole() {
  static char cmd[16];
  static int cmdlen = 0;

  while(Serial.available()) {                     // Read chars from buffer.
    byte c = Serial.read();
    if (KeyboardControl) {
      KeyboardSwitch(c);
      return;
    }
    if (c == 10 || c == 13 || c==64) {
      ExecCommand(cmd);
      cmd[cmdlen = 0] = 0;
      return;
    }
    if (c == '-') {
      cmd[cmdlen = 0] = 0;
      Serial.println("Aborted.");
      return;
    }
    cmd[cmdlen++] = c;
    cmd[cmdlen] = 0;
  }
  return;
}

void ExecCommand(char *cmd) {
  switch (cmd[0]) {
  case 'm':
    ExternalControl = true;
    KeyboardControl = true;
    Serial.println("Entering keyboard control mode.");
    break;
  case 'b':
    cmd++; while (isspace(*cmd)) cmd++;
    config.bearing = atoi(cmd) / 2;
    Serial.print("Set bearing to ");
    Serial.println(config.bearing * 2, DEC);
    break;
  case 'c':
    {
      byte address, value;
      cmd++; while (isspace(*cmd)) cmd++;
      address = strtol(cmd, &cmd, 10);
      while (isspace(*cmd)) cmd++;
      value = strtol(cmd, &cmd, 10);

      byte *buf = config_as_buf(&config);
      buf[address] = value;
      Serial.print("Configuration at ");
      Serial.print(address, DEC);
      Serial.print(" set to ");
      Serial.println(value, DEC);
    }
    break;
  case 'd':
    {
      byte *buf = config_as_buf(&config);
      for (byte address = 0; address < sizeof(config); address++) {
	Serial.print(address, DEC);
	Serial.print(": ");
	Serial.println(buf[address], DEC);
      }
    }
    break;
  case 's':
    config_save();
    Serial.println("Configuration saved.");
    break;
  case 'r':
    config_defaults();
    Serial.println("Configuration reset.");
    break;
  default:
    Serial.println("WTF OMG?");
    break;
  }
  return;
}


void KeyboardSwitch(byte c) {
  switch (c) {
  case '-':
    KeyboardControl = false;
    ExternalControl = false;
    Serial.println("Exit.");
    break;
  case 'a':
    ManualIntensity+=5;
    break;
  case 'z':
    ManualIntensity-=5;
    break;
  case 's':
    config.duration_off+=1;
    break;
  case 'x':
    config.duration_off-=1;
    break;
  case 'd':
    config.duration_on+=1;
    break;
  case 'c':
    config.duration_on-=1;
    break;
  case 'q':
    SwitchBit(0);
    break;
  case 'w':
    SwitchBit(1);
    break;
  case 'e':
    SwitchBit(2);
    break;
  case 'r':
    SwitchBit(3);
    break;
  case 't':
    SwitchBit(4);
    break;
  case 'y':
    SwitchBit(5);
    break;
  case 'u':
    SwitchBit(6);
    break;
  case 'i':
    SwitchBit(7);
    break;
  default:
    Serial.println("WTF?");
    break;
  }
  Serial.print("Running at intensity ");
  Serial.print(ManualIntensity, DEC);
  Serial.print(" (PWM ");
  Serial.print(pwm, DEC);
  Serial.print("), pause ");
  Serial.print(config.duration_off*10, DEC);
  Serial.print(" ms, pulse ");
  Serial.print(config.duration_on*10, DEC);
  Serial.print(" ms, motors ");
  Serial.println(packet, BIN);
}

void SwitchBit (byte pos) {
  if(bitRead(packet, pos)) {
    bitWrite(packet, pos, 0);
  } 
  else {
    bitWrite(packet, pos, 1);
  }
}
</code>