mirror of
https://github.com/felis/USB_Host_Shield_2.0.git
synced 2024-03-22 11:31:26 +01:00
Merge pull request #14 from TKJElectronics/master
Added 360 degrees resolution of angle calculated from accelerometer
This commit is contained in:
commit
92e25ae896
3 changed files with 174 additions and 185 deletions
111
PS3BT.cpp
111
PS3BT.cpp
|
@ -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)
|
||||||
|
|
4
PS3BT.h
4
PS3BT.h
|
@ -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
|
||||||
|
|
|
@ -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));
|
||||||
|
|
Loading…
Reference in a new issue