brmlab

User Tools

Site Tools

You are here: Hackerspace Prague » Projects » BrmPaw » sketch
project:brmpaw:sketch

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

project:brmpaw:sketch [2011/03/04 21:26] – created paskyproject:brmpaw:sketch [2011/03/04 21:28] (current) – pasky
Line 1: Line 1:
 +<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>