Compass Vibro Anklet Code

From Noisebridge
Jump to: navigation, search

Source code for the compass vibro anklet Arduino, as of April 23, 2009.

File name: Compass_Vibro_Anklet_HM55B_090423.pde

/* Skory & Eric
 * Compass Vibro-Anklet
 * We Rule, April 23, 2009
 */


/* Some code from:
 * 2009-03-24, pager motor test, lamont lucas
 */
/*
Some Hitachi HM55B Compass reading code copied from: kiilo kiilo@kiilo.org
License:  http://creativecommons.org/licenses/by-nc-sa/2.5/ch/
 */

/******************************************************************************
 *  i2c_gpio
 *  Keith Neufeld
 *  May 26, 2008
 *
 *  Prototype I2C interface to TI 9535 and 9555 GPIO expanders.
 *
 *  Arduino analog input 5 - I2C SCL
 *  Arduino analog input 4 - I2C SDA
 *
 ******************************************************************************/



// define the pins used to run the shift registers 
int enable_low = 10;  //enable outputs, low = on
int serial_in  = 12; 
int ser_clear_low = 9;  //pulse low to zero out the shift buffer
int RCK  = 7;  //RCK, push the serial buffer to the outputs
int SRCK = 8;  //

#include <math.h>

//// define pins used to operate the digital compass (HM55B)
byte CLK_pin = 11;
byte EN_pin = 5;
byte DIO_pin = 4;

int X_Data = 0;
int Y_Data = 0;
int angle;
unsigned long counter = 0;
int prev_motor = 1;
int curr_motor = 1;
int status;
int cycles_per_second = 23; //board and compass specific - must measure
int count;
int activity = 100;
unsigned long serialTimer = millis();

int MotorStrength = 230;  // 255 = full power 

void setup() {
  pinMode(enable_low, OUTPUT);  // set shift register pins as outputs
  pinMode(serial_in, OUTPUT);
  pinMode(ser_clear_low, OUTPUT);
  pinMode(RCK, OUTPUT);
  pinMode(SRCK, OUTPUT);
  
  // use some serial for debugging
  Serial.begin(115200);
  Serial.println("Setting up board");
  
  // make sure we start out all off
  digitalWrite(enable_low, HIGH);
  // this should wipe out the serial buffer on the shift register
  digitalWrite(ser_clear_low, LOW);
  delay(100);   //delay in ms
  
  // the TPIC6b595 clocks work on a rising edge, so make sure they're low to start.
  digitalWrite(RCK, LOW);
  digitalWrite(SRCK, LOW);
  
  digitalWrite(ser_clear_low, HIGH);   //we are now clear to write into the serial buffer

  Serial.println("Board is setup");

  // setup for HM55B compass chip
  pinMode(EN_pin, OUTPUT);
  pinMode(CLK_pin, OUTPUT);
  pinMode(DIO_pin, INPUT);

  HM55B_Reset();

}


void loop() {
  // make the compass get a reading
  
  HM55B_StartMeasurementCommand(); // necessary!!
  delay(40); // the data is ready 40ms later
  status = HM55B_ReadCommand();
  Serial.print(status); // read data and print Status
  Serial.print(" ");
  X_Data = ShiftIn(11); // Field strength in X
  Y_Data = ShiftIn(11); // and Y direction
  X_Data = X_Data * -1;  // In current rig, chip
  Y_Data = Y_Data * -1;  // is upside-down; compensate
  Serial.print(X_Data); // print X strength
  Serial.print(" ");
  Serial.print(Y_Data); // print Y strength
  Serial.print(" ");
  digitalWrite(EN_pin, HIGH); // ok deselect chip
  angle = 180 * (atan2(-1 * Y_Data , X_Data) / M_PI); // angle is atan( -y/x) !!!
  if (angle < 0) angle = (360 + angle); //offset neg angles
  Serial.print(angle); // print angle
  Serial.println(" ");

  //Turn on the appropriate motor while keeping track of time
  curr_motor = CalcMotor(8, angle);
  if (curr_motor != prev_motor) { //if we changed angle enough
    TurnOnMotor(curr_motor);      //turn on the new motor
    counter = 0;                  //reset counter
    if (activity < 200){
    activity = activity + 1;      //increase activity level
    }                             //mav val = 200
  } else {
    if (counter < (activity / 10) * cycles_per_second) {  //only keep
      TurnOnMotor(curr_motor);   //same motor on for
    } else {                     //less than cycles * activity level
      TurnOnMotor(0);            //
    }
    counter++;                   //increment counter
    if (counter > (600 * cycles_per_second) / activity ){
      counter = 0;               //reset counter
      if (activity > 13){        //lower activity level
      activity = activity - 13;  //max val(s) 0-12
      }
    }
  }
  prev_motor = curr_motor;

  Serial.print("counter: ");
  Serial.print(counter);
  Serial.print(" activity: ");
  Serial.println(activity);
  
/*
//Debug wacky motor wiring disorder  
  count++;
  TurnOnMotor(count);
  Serial.print(count); // print angle
  Serial.println("  ");
  delay(2000);
  if (count >= 8)
  {
    count = 0;
    delay(2000);
  }
*/
  
}



//// FUNCTIONS

void TurnOnMotor(int which){
  // accept which from 1 to 8
  // send message to shift register as appropiate
  digitalWrite(enable_low, HIGH);
  delayMicroseconds(100);  //slow and steady
  Serial.print("Motor  ");
  Serial.println(which); // print angle
  switch(which){
    case 8:
      shiftOut(serial_in, SRCK, LSBFIRST, B00000100);
      break;
    case 1:
      shiftOut(serial_in, SRCK, LSBFIRST, B00000001);
      break;
    case 2:
      shiftOut(serial_in, SRCK, LSBFIRST, B00001000);
      break;
    case 3:
      shiftOut(serial_in, SRCK, LSBFIRST, B00000010);
      break;
    case 4:
      shiftOut(serial_in, SRCK, LSBFIRST, B00010000);
      break;
    case 5:
      shiftOut(serial_in, SRCK, LSBFIRST, B00100000);
      break;
    case 6:
      shiftOut(serial_in, SRCK, LSBFIRST, B01000000);
      break;
    case 7:
      shiftOut(serial_in, SRCK, LSBFIRST, B10000000);
      break;
    case 9:
      shiftOut(serial_in, SRCK, LSBFIRST, B11111111);
      break;
    case 10:
      shiftOut(serial_in, SRCK, LSBFIRST, B11110000);
      break;
    case 11:
      shiftOut(serial_in, SRCK, LSBFIRST, B00001111);
      break;
    default:
      // turn them all off
      shiftOut(serial_in, SRCK, LSBFIRST, B00000000);
  } 
  //in all cases, pulse RCK to pop that into the outputs
  delayMicroseconds(100);
  digitalWrite(RCK, HIGH);
  delayMicroseconds(100);
  digitalWrite(RCK, LOW);
  analogWrite(enable_low, 255-MotorStrength);
}




int CalcAngle(int howMany, int which)
{  // function which calculates the "switch to next motor" angle
  // given how many motors there are in a circle and which position you want
  // assume which is 1-indexed (i.e. first position is 1, not zero)
  // assume circle is 0-360, we can always offset later...
  
  return (360/howMany*(which-0.5));
}

int CalcMotor(int howMany, int angle)
{  // function to calculate which motor to turn on, given
  // how many motors there are and what the current angle is
  // assumes motor 1 = angle 0
  // assumes angle is from 0-360
  int i;
  for (i = 1; i<howMany;i++)
  {
    if ( (angle >= CalcAngle(howMany, i)) & (angle <= CalcAngle(howMany, i+1)) )
       return i+1; 
  } 
  // if we're still here, it's the last case, the loop over case, which
  // is actually motor 1 by assumption
  return 1;
}




//HM55B Functions

void ShiftOut(int Value, int BitsCount) {
  for(int i = BitsCount; i >= 0; i--) {
    digitalWrite(CLK_pin, LOW);
    if ((Value & 1 << i) == ( 1 << i)) {
      digitalWrite(DIO_pin, HIGH);
      //Serial.print("1");
    }
    else {
      digitalWrite(DIO_pin, LOW);
      //Serial.print("0");
    }
    digitalWrite(CLK_pin, HIGH);
    delayMicroseconds(1);
  }
}

int ShiftIn(int BitsCount) {
  int ShiftIn_result;
    ShiftIn_result = 0;
    pinMode(DIO_pin, INPUT);
    for(int i = BitsCount; i >= 0; i--) {
      digitalWrite(CLK_pin, HIGH);
      delayMicroseconds(1);
      if (digitalRead(DIO_pin) == HIGH) {
        ShiftIn_result = (ShiftIn_result << 1) + 1; 
      }
      else {
        ShiftIn_result = (ShiftIn_result << 1) + 0;
      }
      digitalWrite(CLK_pin, LOW);
      delayMicroseconds(1);
    }
  //Serial.print(":");

// below is difficult to understand:
// if bit 11 is Set the value is negative
// the representation of negative values you
// have to add B11111000 in the upper Byte of
// the integer.
// see: http://en.wikipedia.org/wiki/Two%27s_complement
  if ((ShiftIn_result & 1 << 11) == 1 << 11) {
    ShiftIn_result = (B11111000 << 8) | ShiftIn_result; 
  }


  return ShiftIn_result;
}

void HM55B_Reset() {
  pinMode(DIO_pin, OUTPUT);
  digitalWrite(EN_pin, LOW);
  ShiftOut(B0000, 3);
  digitalWrite(EN_pin, HIGH);
}

void HM55B_StartMeasurementCommand() {
  pinMode(DIO_pin, OUTPUT);
  digitalWrite(EN_pin, LOW);
  ShiftOut(B1000, 3);
  digitalWrite(EN_pin, HIGH);
}

int HM55B_ReadCommand() {
  int result = 0;
  pinMode(DIO_pin, OUTPUT);
  digitalWrite(EN_pin, LOW);
  ShiftOut(B1100, 3);
  result = ShiftIn(3);
  return result;
}


Personal tools