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:
Oleg Mazurov
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_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
|
||||
|
|
@ -483,7 +482,7 @@ uint8_t PS3BT::getAnalogHat(AnalogHat a)
|
|||
return 0;
|
||||
return (uint8_t)(l2capinbuf[(uint16_t)a]);
|
||||
}
|
||||
uint32_t PS3BT::getSensor(Sensor a)
|
||||
int32_t PS3BT::getSensor(Sensor a)
|
||||
{
|
||||
if (l2capinbuf == NULL)
|
||||
return 0;
|
||||
|
|
@ -505,83 +504,63 @@ uint32_t PS3BT::getSensor(Sensor a)
|
|||
return 0;
|
||||
}
|
||||
else if (a == tempMove)
|
||||
{
|
||||
if (l2capinbuf == NULL)
|
||||
return 0;
|
||||
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
|
||||
{
|
||||
|
||||
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 PS3BT::getAngle(Angle a) {
|
||||
double accXval;
|
||||
double angleX;
|
||||
|
||||
double accYin;
|
||||
double accYval;
|
||||
double angleY;
|
||||
|
||||
double accZin;
|
||||
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 zeroG = 511.5; // 1.65/3.3*1023 (1,65V)
|
||||
double R;//force vector
|
||||
|
||||
accXin = getSensor(aX);
|
||||
accXval = (zeroG - accXin) / sensivity;//Convert to g's
|
||||
accXval = ((double)getSensor(aX)-zeroG) / sensivity; // Convert to g's
|
||||
accXval *= 2;
|
||||
|
||||
accYin = getSensor(aY);
|
||||
accYval = (zeroG - accYin) / sensivity;//Convert to g's
|
||||
accYval = ((double)getSensor(aY)-zeroG) / sensivity; // Convert to g's
|
||||
accYval *= 2;
|
||||
|
||||
accZin = getSensor(aZ);
|
||||
accZval = (zeroG - accZin) / sensivity;//Convert to g's
|
||||
accZval = ((double)getSensor(aZ)-zeroG) / sensivity; // Convert to g's
|
||||
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)
|
||||
{
|
||||
//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;
|
||||
if(accYval < -1) // Convert to g's
|
||||
accYval = ((1+accYval)-(1-1.15))*(-1/0.15);
|
||||
else if(accYval > 1)
|
||||
accYval = ((1+accYval)-(1+1.15))*(-1/0.15);
|
||||
|
||||
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
|
||||
{
|
||||
//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;
|
||||
|
||||
double R = sqrt(accXval*accXval + accYval*accYval + accZval*accZval); // Calculate the length of the force vector
|
||||
// Normalize vectors
|
||||
accXval = accXval/R;
|
||||
accYval = accYval/R;
|
||||
accZval = accZval/R;
|
||||
|
||||
// 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(-accYval,-accZval)+PI)*RAD_TO_DEG;
|
||||
return angle;
|
||||
} else {
|
||||
double angle = (atan2(-accXval,-accZval)+PI)*RAD_TO_DEG;
|
||||
return angle;
|
||||
}
|
||||
}
|
||||
bool PS3BT::getStatus(Status c)
|
||||
|
|
|
|||
4
PS3BT.h
4
PS3BT.h
|
|
@ -338,8 +338,8 @@ public:
|
|||
bool getButton(Button b);
|
||||
uint8_t getAnalogButton(AnalogButton a);
|
||||
uint8_t getAnalogHat(AnalogHat a);
|
||||
uint32_t getSensor(Sensor a);
|
||||
double getAngle(Angle a, boolean resolution);
|
||||
int32_t getSensor(Sensor a);
|
||||
double getAngle(Angle a);
|
||||
bool getStatus(Status c);
|
||||
String getStatusString();
|
||||
void disconnect(); // use this void to disconnect any of the controllers
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -142,10 +142,14 @@ void loop()
|
|||
} else {
|
||||
if(BT.getButton(SELECT_MOVE)) {
|
||||
Serial.print(F(" - Select"));
|
||||
printTemperature = false;
|
||||
printTemperature = !printTemperature;
|
||||
while(BT.getButton(SELECT_MOVE))
|
||||
Usb.Task();
|
||||
} if(BT.getButton(START_MOVE)) {
|
||||
Serial.print(F(" - Start"));
|
||||
printTemperature = true;
|
||||
printAngle = !printAngle;
|
||||
while(BT.getButton(START_MOVE))
|
||||
Usb.Task();
|
||||
} if(BT.getButton(TRIANGLE_MOVE)) {
|
||||
Serial.print(F(" - Triangle"));
|
||||
BT.moveSetBulb(Red);
|
||||
|
|
@ -170,7 +174,13 @@ void loop()
|
|||
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 temphigh;
|
||||
String input = String(BT.getSensor(tempMove));
|
||||
|
|
|
|||
Loading…
Reference in a new issue