Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
1 contributor

Users who have contributed to this file

317 lines (285 sloc) 8.29 KB
#include <Wire.h>
#define STATUS 13
#define BUTTON 2
#define CALIBRATE_BUTTON 3
#define POT A0
#define RUMBLE 12
#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C
#define GYRO_FULL_SCALE_250_DPS 0x00
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18
#define ACC_FULL_SCALE_2_G 0x00
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18
#define ROTATION_COMMAND 'R'
#define ZOOM_COMMAND 'Z'
#define BUTTON_DOWN_COMMAND 'D'
#define BUTTON_UP_COMMAND 'U'
#define CALIBRATE_COMMAND 'C'
#define COLOR_COMMAND "COL"
bool connectedToClient = false;
static bool isButtonDown = false;
int shortBuzzTime = 25;
unsigned long buzzMillis = 0;
struct Vector3
{
int16_t x;
int16_t y;
int16_t z;
};
byte RGBPins[3] = {11,10,9};
Vector3 localRGBColor = {255,255,255};
Vector3 gameRGBColor = {255,255,255};
float redBias = 1;
float greenBias = 0.25;
float blueBias = 0.25;
//Equality operator overloads for Vector3
inline bool operator==(const Vector3& lhs, const Vector3& rhs){ if(lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z)return true; else return false; }
inline bool operator!=(const Vector3& lhs, const Vector3& rhs){ return !(lhs == rhs); }
void setup()
{
Wire.begin();
Serial.begin(115200);
pinMode(STATUS, OUTPUT);
pinMode(RUMBLE, OUTPUT);
pinMode(POT, INPUT);
pinMode(BUTTON, INPUT_PULLUP);
pinMode(CALIBRATE_BUTTON, INPUT_PULLUP);
for(int i = 0; i<3; i++) pinMode(RGBPins[i], OUTPUT);
attachInterrupt(digitalPinToInterrupt(BUTTON), SendButton, CHANGE);
attachInterrupt(digitalPinToInterrupt(CALIBRATE_BUTTON), SendCalibrate, LOW);
// Configure gyroscope range
I2CwriteByte(MPU9250_ADDRESS,27,GYRO_FULL_SCALE_2000_DPS);
// Configure accelerometers range
I2CwriteByte(MPU9250_ADDRESS,28,ACC_FULL_SCALE_16_G);
}
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.endTransmission();
// Read Nbytes
Wire.requestFrom(Address, Nbytes);
uint8_t index=0;
while (Wire.available())
Data[index++]=Wire.read();
}
void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
// Set register address
Wire.beginTransmission(Address);
Wire.write(Register);
Wire.write(Data);
Wire.endTransmission();
}
bool buttonDebounce = false;
int16_t rgbCount = 250;
void loop()
{
while (!connectedToClient) //Wait for client hand-shake
{
if (Serial.available() > 0)
{
String incomming = Serial.readString();
if (incomming == "DISCOVER")
{
Serial.println("ACKNOWLEDGE");
digitalWrite(STATUS, HIGH);
connectedToClient = true;
isButtonDown = !digitalRead(BUTTON);
}
}
}
if (connectedToClient)
{
//Recieve commands
if (Serial.available() > 0)
{
String incomming = Serial.readString();
if (incomming == "DISCOVER") //If the client decides to reconnect, do not ignore the DISCOVER command.
{
Serial.println("ACKNOWLEDGE");
digitalWrite(STATUS, HIGH);
connectedToClient = true;
isButtonDown = !digitalRead(BUTTON);
}
else if(incomming == "TERMINATE")
{
digitalWrite(STATUS, LOW);
connectedToClient = false;
}
else
{
ParseIncommingCommand(incomming);
}
}
//Send Commands
bool isBuzzing = buzzMillis+shortBuzzTime > millis();
if(!isBuzzing)
{
SendGyroData();
SendPotValue();
}
digitalWrite(RUMBLE, isBuzzing);
if(localRGBColor != gameRGBColor)
{
FadeColors();
}
if(rgbCount >= 300)
{
GenerateColor();
rgbCount = 0;
}
delay(16.2); //Delay to match 60 updates per second. Otherwise, the serial bus would be flooded with commands.
isButtonDown = !digitalRead(BUTTON); //Even though the button is on an interupt system, the pin still needs to be checked to ensure the interupt has the correct button state.
rgbCount++;
}
}
void FadeColors()
{
StepInt(localRGBColor.x, gameRGBColor.x*redBias, 5);
StepInt(localRGBColor.y, gameRGBColor.y*greenBias, 5);
StepInt(localRGBColor.z, gameRGBColor.z*blueBias, 5);
analogWrite(RGBPins[0], localRGBColor.x); //Red
analogWrite(RGBPins[1], localRGBColor.y); //Green
analogWrite(RGBPins[2], localRGBColor.z); //Blue
}
void StepInt(int16_t &a, int16_t b, uint16_t rate)
{
if(a == b)
return;
if(b > a)
a+=rate;
else
a-=rate;
if(a>=255) a = 255;
if(a<=0) a = 0;
}
int lastPtValue = 1;
void SendPotValue()
{
int ptValue = analogRead(POT);
int z = map(ptValue, 0, 1024, 15, 60);
if(z != lastPtValue)
{
Serial.print(ZOOM_COMMAND);
Serial.print(':');
Serial.print(z);
Serial.println("");
lastPtValue = z;
}
}
Vector3 lastAccl;
void SendGyroData()
{
// Read accelerometer and gyroscope
uint8_t Buf[14];
I2Cread(MPU9250_ADDRESS,0x3B,14,Buf);
Vector3 accl;
// Create 16 bits values from 8 bits data
// Accelerometer
accl.x=-(Buf[0]<<8 | Buf[1]);
accl.y=-(Buf[2]<<8 | Buf[3]);
accl.z= Buf[4]<<8 | Buf[5];
accl.x = map(accl.x, -3000, 3000, -360, 360);
accl.y = map(accl.y, -3000, 3000, -360, 360);
accl.z = map(accl.z, -3000, 3000, -360, 360);
// Gyroscope
int16_t gx=-(Buf[8]<<8 | Buf[9]);
int16_t gy=-(Buf[10]<<8 | Buf[11]);
int16_t gz=Buf[12]<<8 | Buf[13];
if(VectorDistance(accl, lastAccl) > 5) //Compress accl data before sending
{
// Accelerometer Out
Serial.print(ROTATION_COMMAND);
Serial.print (accl.x,DEC);
Serial.print (":");
Serial.print (accl.y,DEC);
Serial.print (":");
Serial.print (accl.z,DEC);
Serial.println("");
lastAccl = accl;
}
//delay(16.6); //Delay for 60 updates per second.
}
float VectorDistance(Vector3 a, Vector3 b)
{
return sqr( pow((b.x-a.x),2) + pow((b.y-a.y),2) + pow((b.z-a.z),2));
}
int sqr(int x) //Fast square Root
{
int s, t;
s = 1; t = x;
while (s < t) {
s <<= 1;
t >>= 1;
}
do {
t = s;
s = (x / s + s) >> 1;
} while (s < t);
return t;
}
void SendButton()
{
static unsigned long last_interrupt_time = 0;
unsigned long interrupt_time = millis();
if (interrupt_time - last_interrupt_time > 10) //Debounce check
{
Serial.println(isButtonDown == false ? BUTTON_DOWN_COMMAND : BUTTON_UP_COMMAND);
buzzMillis = interrupt_time;
isButtonDown = !isButtonDown;
}
last_interrupt_time = interrupt_time;
}
void SendCalibrate()
{
static unsigned long last_interrupt_time2 = 0;
unsigned long interrupt_time = millis();
if (interrupt_time - last_interrupt_time2 > 10) //Debounce check
{
Serial.println(CALIBRATE_COMMAND);
}
last_interrupt_time2 = interrupt_time;
}
void ParseIncommingCommand(String inc) //Send commands in C# using SerialPort.Write, not SerialPort.WriteLine!
{ //Send C# commands in this format PIN:INPUTOUTPUT:HIGHLOW
//EX: 12:1:1 (Set pin 12 to output and set to high)
//Serial.print(millis()/1000);
//Serial.print("s RECEIVED COMMAND: ");
//Serial.println(inc);
String cmd1 = GetValue(inc, ':', 0);
String cmd2 = GetValue(inc, ':', 1);
String cmd3 = GetValue(inc, ':', 2);
int pin = cmd1.toInt();
pinMode(pin, cmd2 == "0" ? INPUT : OUTPUT);
pinMode(pin, cmd3 == "1" ? HIGH : LOW);
}
void GenerateColor()
{
randomSeed(millis());
gameRGBColor.x = random(0,255);
gameRGBColor.y = random(0,255);
gameRGBColor.z = random(0,255);
Serial.print(COLOR_COMMAND); Serial.print(gameRGBColor.x);
Serial.print(":"); Serial.print(gameRGBColor.y);
Serial.print(":"); Serial.print(gameRGBColor.z); Serial.println("");
}
String GetValue(String data, char separator, int index) //split strings by delimiters
{
int found = 0;
int strIndex[] = {0, -1 };
int maxIndex = data.length() - 1;
for (int i = 0; i <= maxIndex && found <= index; i++) {
if (data.charAt(i) == separator || i == maxIndex) {
found++;
strIndex[0] = strIndex[1] + 1;
strIndex[1] = (i == maxIndex) ? i + 1 : i;
}
}
return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
}
You can’t perform that action at this time.