Compass Vibro Anklet Code
Jump to navigation
Jump to 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;
}