specchioriflesso/USB_Host_Shield_2.0
1
Fork 0
mirror of https://github.com/felis/USB_Host_Shield_2.0.git synced 2024-03-22 11:31:26 +01:00
Code Issues Projects Releases Packages Wiki Activity

Added 360 resolution

Browse source
This commit is contained in:
Kristian Lauszus 2012-04-24 01:35:43 +02:00
parent fd58db2680
commit 98cf17c947
3 changed files with 131 additions and 171 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

84
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_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,
0xff, 0x27, 0x10, 0x00, 0x32,
@ -37,7 +36,7 @@ prog_char OUTPUT_REPORT_BUFFER[] PROGMEM =
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
bAddress(0), // device address - mandatory
bNumEP(1), // if config descriptor needs to be parsed
@ -516,72 +515,33 @@ uint32_t PS3BT::getSensor(Sensor 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 angleX;
double accYin;
double accYval;
double angleY;
double accZin;
double accZval;
double PS3BT::getAngle(Angle a) {
// Data for the Kionix KXPC4 used in DualShock 3
double sensivity = 204.6;//0.66/3.3*1023 (660mV/g)
double zeroG = 511.5;//1.65/3.3*1023 (1,65V)
double R;//force vector
const double sensivity = 204.6; // 0.66/3.3*1023 (660mV/g)
const double zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
accXin = getSensor(aX);
accXval = (zeroG - accXin) / sensivity;//Convert to g's
double accXval = (zeroG - getSensor(aX)) / sensivity; // Convert to g's
accXval *= 2;
accYin = getSensor(aY);
accYval = (zeroG - accYin) / sensivity;//Convert to g's
double accYval = (zeroG - getSensor(aY)) / sensivity; // Convert to g's
accYval *= 2;
accZin = getSensor(aZ);
accZval = (zeroG - accZin) / sensivity;//Convert to g's
double accZval = (zeroG - getSensor(aZ)) / sensivity; // Convert to g's
accZval *= 2;
R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2));
double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval);
// convert read values to -π/2 to π/2 - Needed for atan2
double angleX = acos(accXval/R)-0.5*PI;
double angleY = acos(accYval/R)-0.5*PI;
double angleZ = acos(accZval/R)-0.5*PI;
if (a == Pitch)
{
//the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees
//In the end it is minus by 90, so its 0 degrees when in horizontal postion
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;
}
else
{
//the result will come out as radians, so it is multiplied by 180/pi, to convert to degrees
//In the end it is minus by 90, so its 0 degrees when in horizontal postion
angleY = acos(accYval / 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 (angleY < 0)
angleY = -180 - angleY;
else
angleY = 180 - angleY;
}
}
return angleY;
// Convert to 360 degrees resolution
// atan2 outputs the value of -π to π (radians)
// We are then converting it to 0 to 2π and then to degrees
if (a == Pitch) {
double angle = (atan2(-angleY,-angleZ)+PI)*RAD_TO_DEG;
return angle;
} else {
double angle = (atan2(-angleX,-angleZ)+PI)*RAD_TO_DEG;
return angle;
}
}
bool PS3BT::getStatus(Status c)

2
PS3BT.h
View file

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

4
examples/PS3BT/PS3BT.ino
View file

@ -123,9 +123,9 @@ void loop()
}
if(printAngle) {
Serial.print(F("Pitch: "));
Serial.print(BT.getAngle(Pitch,false));
Serial.print(BT.getAngle(Pitch));
Serial.print(F("\tRoll: "));
Serial.println(BT.getAngle(Roll,false));
Serial.println(BT.getAngle(Roll));
}
}
else if(BT.PS3MoveBTConnected)