Merge pull request #14 from TKJElectronics/master

Added 360 degrees resolution of angle calculated from accelerometer
This commit is contained in:
Oleg Mazurov 2012-04-24 13:53:11 -07:00
commit 92e25ae896
3 changed files with 174 additions and 185 deletions

111
PS3BT.cpp
View file

@ -24,8 +24,7 @@ const uint8_t PS3BT::BTD_EVENT_PIPE = 1;
const uint8_t PS3BT::BTD_DATAIN_PIPE = 2; const uint8_t PS3BT::BTD_DATAIN_PIPE = 2;
const uint8_t PS3BT::BTD_DATAOUT_PIPE = 3; const uint8_t PS3BT::BTD_DATAOUT_PIPE = 3;
prog_char OUTPUT_REPORT_BUFFER[] PROGMEM = prog_char OUTPUT_REPORT_BUFFER[] PROGMEM = {
{
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0xff, 0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00, 0x32,
@ -37,7 +36,7 @@ prog_char OUTPUT_REPORT_BUFFER[] PROGMEM =
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
}; };
PS3BT::PS3BT(USB *p, uint8_t btadr5 = 0, uint8_t btadr4 = 0, uint8_t btadr3 = 0, uint8_t btadr2 = 0, uint8_t btadr1 = 0, uint8_t btadr0 = 0): PS3BT::PS3BT(USB *p, uint8_t btadr5, uint8_t btadr4, uint8_t btadr3, uint8_t btadr2, uint8_t btadr1, uint8_t btadr0):
pUsb(p), // pointer to USB class instance - mandatory pUsb(p), // pointer to USB class instance - mandatory
bAddress(0), // device address - mandatory bAddress(0), // device address - mandatory
bNumEP(1), // if config descriptor needs to be parsed bNumEP(1), // if config descriptor needs to be parsed
@ -483,7 +482,7 @@ uint8_t PS3BT::getAnalogHat(AnalogHat a)
return 0; return 0;
return (uint8_t)(l2capinbuf[(uint16_t)a]); return (uint8_t)(l2capinbuf[(uint16_t)a]);
} }
uint32_t PS3BT::getSensor(Sensor a) int32_t PS3BT::getSensor(Sensor a)
{ {
if (l2capinbuf == NULL) if (l2capinbuf == NULL)
return 0; return 0;
@ -505,83 +504,63 @@ uint32_t PS3BT::getSensor(Sensor a)
return 0; return 0;
} }
else if (a == tempMove) else if (a == tempMove)
{
if (l2capinbuf == NULL)
return 0;
return (((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4) | (l2capinbuf[(uint16_t)a] << 4)); return (((l2capinbuf[(uint16_t)a + 1] & 0xF0) >> 4) | (l2capinbuf[(uint16_t)a] << 4));
else // aXmove, aYmove, aZmove, gXmove, gYmove and gZmove
return ((l2capinbuf[(uint16_t)a + 1] << 8) | l2capinbuf[(uint16_t)a]);
} }
else double PS3BT::getAngle(Angle a) {
{
return (((l2capinbuf[(uint16_t)a + 1] << 8) | l2capinbuf[(uint16_t)a]) - 0x8000);
}
}
double PS3BT::getAngle(Angle a, boolean resolution) // Boolean indicate if 360-degrees resolution is used or not - set false if you want to use both axis
{
double accXin;
double accXval; double accXval;
double angleX;
double accYin;
double accYval; double accYval;
double angleY;
double accZin;
double accZval; double accZval;
//Data for the Kionix KXPC4 used in DualShock 3 if(PS3BTConnected) {
// Data for the Kionix KXPC4 used in the DualShock 3
double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g) double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g)
double zeroG = 511.5; // 1.65/3.3*1023 (1,65V) double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
double R;//force vector accXval = ((double)getSensor(aX)-zeroG) / sensivity; // Convert to g's
accXin = getSensor(aX);
accXval = (zeroG - accXin) / sensivity;//Convert to g's
accXval *= 2; accXval *= 2;
accYval = ((double)getSensor(aY)-zeroG) / sensivity; // Convert to g's
accYin = getSensor(aY);
accYval = (zeroG - accYin) / sensivity;//Convert to g's
accYval *= 2; accYval *= 2;
accZval = ((double)getSensor(aZ)-zeroG) / sensivity; // Convert to g's
accZin = getSensor(aZ);
accZval = (zeroG - accZin) / sensivity;//Convert to g's
accZval *= 2; accZval *= 2;
} else if(PS3MoveBTConnected) {
// It's a Kionix KXSC4 inside the Motion controller
const uint16_t sensivity = 28285; // Find by experimenting
accXval = (double)getSensor(aXmove)/sensivity;
accYval = (double)getSensor(aYmove)/sensivity;
accZval = (double)getSensor(aZmove)/sensivity;
R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); if(accXval < -1) // Convert to g's
accXval = ((1+accXval)-(1-1.15))*(-1/0.15);
else if(accXval > 1)
accXval = ((1+accXval)-(1+1.15))*(-1/0.15);
if (a == Pitch) if(accYval < -1) // Convert to g's
{ accYval = ((1+accYval)-(1-1.15))*(-1/0.15);
//the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees else if(accYval > 1)
//In the end it is minus by 90, so its 0 degrees when in horizontal postion accYval = ((1+accYval)-(1+1.15))*(-1/0.15);
angleX = acos(accXval / R) * 180 / PI - 90;
if(resolution)
{
if (accZval < 0)//Convert to 360 degrees resolution - set resolution false if you need both pitch and roll
{
if (angleX < 0)
angleX = -180 - angleX;
else
angleX = 180 - angleX;
}
}
return angleX;
if(accZval < -1) // Convert to g's
accZval = ((1+accZval)-(1-1.15))*(-1/0.15);
else if(accZval > 1)
accZval = ((1+accZval)-(1+1.15))*(-1/0.15);
} }
else
{ double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval); // Calculate the length of the force vector
//the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees // Normalize vectors
//In the end it is minus by 90, so its 0 degrees when in horizontal postion accXval = accXval/R;
angleY = acos(accYval / R) * 180 / PI - 90; accYval = accYval/R;
if(resolution) accZval = accZval/R;
{
if (accZval < 0)//Convert to 360 degrees resolution - set resolution false if you need both pitch and roll // Convert to 360 degrees resolution
{ // atan2 outputs the value of -π to π (radians)
if (angleY < 0) // We are then converting it to 0 to 2π and then to degrees
angleY = -180 - angleY; if (a == Pitch) {
else double angle = (atan2(-accYval,-accZval)+PI)*RAD_TO_DEG;
angleY = 180 - angleY; return angle;
} } else {
} double angle = (atan2(-accXval,-accZval)+PI)*RAD_TO_DEG;
return angleY; return angle;
} }
} }
bool PS3BT::getStatus(Status c) bool PS3BT::getStatus(Status c)

View file

@ -338,8 +338,8 @@ public:
bool getButton(Button b); bool getButton(Button b);
uint8_t getAnalogButton(AnalogButton a); uint8_t getAnalogButton(AnalogButton a);
uint8_t getAnalogHat(AnalogHat a); uint8_t getAnalogHat(AnalogHat a);
uint32_t getSensor(Sensor a); int32_t getSensor(Sensor a);
double getAngle(Angle a, boolean resolution); double getAngle(Angle a);
bool getStatus(Status c); bool getStatus(Status c);
String getStatusString(); String getStatusString();
void disconnect(); // use this void to disconnect any of the controllers void disconnect(); // use this void to disconnect any of the controllers

View file

@ -123,9 +123,9 @@ void loop()
} }
if(printAngle) { if(printAngle) {
Serial.print(F("Pitch: ")); Serial.print(F("Pitch: "));
Serial.print(BT.getAngle(Pitch,false)); Serial.print(BT.getAngle(Pitch));
Serial.print(F("\tRoll: ")); Serial.print(F("\tRoll: "));
Serial.println(BT.getAngle(Roll,false)); Serial.println(BT.getAngle(Roll));
} }
} }
else if(BT.PS3MoveBTConnected) else if(BT.PS3MoveBTConnected)
@ -142,10 +142,14 @@ void loop()
} else { } else {
if(BT.getButton(SELECT_MOVE)) { if(BT.getButton(SELECT_MOVE)) {
Serial.print(F(" - Select")); Serial.print(F(" - Select"));
printTemperature = false; printTemperature = !printTemperature;
while(BT.getButton(SELECT_MOVE))
Usb.Task();
} if(BT.getButton(START_MOVE)) { } if(BT.getButton(START_MOVE)) {
Serial.print(F(" - Start")); Serial.print(F(" - Start"));
printTemperature = true; printAngle = !printAngle;
while(BT.getButton(START_MOVE))
Usb.Task();
} if(BT.getButton(TRIANGLE_MOVE)) { } if(BT.getButton(TRIANGLE_MOVE)) {
Serial.print(F(" - Triangle")); Serial.print(F(" - Triangle"));
BT.moveSetBulb(Red); BT.moveSetBulb(Red);
@ -170,7 +174,13 @@ void loop()
Serial.println(""); Serial.println("");
} }
} }
if(printTemperature) { if(printAngle) {
Serial.print(F("Pitch: "));
Serial.print(BT.getAngle(Pitch));
Serial.print(F("\tRoll: "));
Serial.println(BT.getAngle(Roll));
}
else if(printTemperature) {
String templow; String templow;
String temphigh; String temphigh;
String input = String(BT.getSensor(tempMove)); String input = String(BT.getSensor(tempMove));