Hello,everybody, I have some trouble with my code,it can not work well by PS3 controller,when I pressed the ‘cross’, the feedback will be ‘circle’,
Could anyone tell me why?
Thank you!(I use STM32F446)
This is my PS3.h
#ifndef PS3_H
#define PS3_H
#include <mbed.h>
#define PI 3.141592654
/**
- handle PS3 for Mbed OS6
/
class PS3 : public UnbufferedSerial {
public:
// bit,button
enum Button_type {
sikaku = 16, // 00010000
sankaku = 36, // 00100100
batu = 37, // 00100101
maru = 38, // 00100110
ue = 32, // 00100000
sita = 33, // 00100001
migi = 34, // 00100010
hidari = 35, // 00100011
L1 = 17, // 00010001
L2 = 18, // 00010010
R1 = 19, // 00010011
R2 = 20 // 00010100
};
/- Constructor
- Create a UnbufferedSerial port, connected to the specified transmit and
- receive pins,
- @param tx Transmit pin
-
@param rx Receive pin
/
PS3(PinName TX, PinName RX);
/ - Read the contents of PS3’s data into PS3_data
/
void getdata();
/ - create attatch
/
void myattach();
/ - add function after PS3::getdata
-
@param Func Function to call afterPS3::getdata
*/
void addattach(void (Func)());
/ - do nothing
/
void nothingFunc();
/ - Cheak if button in pushed
- @param button which button cheak (e.g. maru,ur,sita,L1 )
-
@return True for button is pushed , False for button is not pushed
/
bool getButtonState(Button_type button);
/ - Cheak if select button in pushed
- @return True for select button is pushed , False for select button is not
- pushed
/
bool getSELECTState();
/ - Cheak if start button in pushed
- @return True for start button is pushed , False for start button is not
- pushed
**/
bool getSTARTState();
/**
- get right joystick’s X axis
-
@return value of right joystick’s X axis (take between -64 and 63)
/
int getRightJoystickXaxis();
/ - get right joystick’s Y axis
-
@return value of right joystick’s Y axis (take between -63 and 64)
/
int getRightJoystickYaxis();
/ - get left joystick’s X axis
-
@return value of left joystick’s X axis (take between -64 and 63)
/
int getLeftJoystickXaxis();
/ - get left joystick’s Y axis
-
@return value of left joystick’s Y axis (take between -63 and 64)
**/
int getLeftJoystickYaxis();
/**
- get right joystick’s angle in degree
-
@return angle of right joystick in degree (take beteen -180.0 and 180.0)
/
double getRightJoystickAngle();
/ - get left joystick’s angle in degree
-
@return angle of left joystick in degree (take beteen -180.0 and 180.0)
/
double getLeftJoystickAngle();
/ - print PS3Data in console
**/
void printdata();
private:
char PS3Data[8];
void (*fpFunc)();
bool addflag;
};
#endif
This is my PS3.cpp
#include “PS3.h”
#include <mbed.h>
void PS3::getdata() {
char read_data;
if (UnbufferedSerial::readable()) {
while (UnbufferedSerial::read(&read_data, 1), read_data != 0x80)
;
for (int i = 1; i < 8; i++) {
UnbufferedSerial::read(&read_data, 1);
PS3Data[i] = read_data;
}
}
if (addflag)
(*fpFunc)();
}
PS3::PS3(PinName TX, PinName RX) : UnbufferedSerial(TX, RX) {
PS3Data[0] = 128; // 0x80
PS3Data[1] = 0;
PS3Data[2] = 0;
PS3Data[3] = 64;
PS3Data[4] = 64;
PS3Data[5] = 64;
PS3Data[6] = 64;
PS3Data[7] = 0;
addflag = 0;
UnbufferedSerial::baud(9600);
// UnbufferedSerial::format(8,None,1);
UnbufferedSerial::attach(Callback<void()>(this, &PS3::getdata), RxIrq);
}
void PS3::myattach() {
UnbufferedSerial::attach(Callback<void()>(this, &PS3::getdata), RxIrq);
}
void PS3::addattach(void (*Func)()) {
fpFunc = Func;
addflag = 1;
}
void PS3::nothingFunc() {}
bool PS3::getButtonState(Button_type button) {
return (*(PS3Data + (button >> 4)) >> (button & 0x0f)) & 1;
}
bool PS3::getSELECTState() {
return getButtonState(migi) & getButtonState(hidari);
}
bool PS3::getSTARTState() { return getButtonState(ue) & getButtonState(sita); }
int PS3::getRightJoystickXaxis() { return (int)PS3Data[5] - 64; }
int PS3::getRightJoystickYaxis() { return (int)PS3Data[6] * -1 + 64; }
int PS3::getLeftJoystickXaxis() { return (int)PS3Data[3] - 64; }
int PS3::getLeftJoystickYaxis() { return (int)PS3Data[4] * -1 + 64; }
double PS3::getRightJoystickAngle() {
return atan2(double(PS3Data[6] * -1 + 64), double(PS3Data[5] - 64)) *
double(180 / PI);
}
double PS3::getLeftJoystickAngle() {
return atan2(double(PS3Data[4] * -1 + 64), double(PS3Data[3] - 64)) *
double(180 / PI);
}
void PS3::printdata() {
for (int i = 0; i < 8; i++) {
printf(“%4d”, PS3Data[i]);
}
printf(“\n”);
}
This is my main.cpp
#include “mbed.h”
#include “math.h”
#include “QEI.h”
#include “PS3.h”
//pcと通信
// UnbufferedSerial pc(USBTX, USBRX);
// 定义ENC1(编码器)
QEI wheel1(PA_5, PC_9, NC, 400);
// 定义ENC2(编码器)
QEI wheel2(PA_4, PB_0, NC, 400);
// 定义MD1D(马达)
DigitalOut MD1D(PA_6);
PwmOut MD1P(PB_6);
// 定义MD2D(马达)
DigitalOut MD2D(PA_9);
PwmOut MD2P(PA_8);
// 定义MD3D(马达)
DigitalOut MD3D(PB_2);
PwmOut MD3P(PB_1);
// 定义MD4D(马达)
DigitalOut MD4D(PB_15);
PwmOut MD4P(PB_14);
// 遥控器
PS3 sbdbt(PC_12, PD_2);
int circle, triangle, square, cross, right, up, left, down, LEFT1, RIGHT1, LEFT2, RIGHT2,
R_JOY_X, R_JOY_Y, L_JOY_X, L_JOY_Y, SELECT, START;
void PS3_Button(void);
void PS3_Check(void);
double mdp[6];
int mdd[6];
void drivemotor(void);
int main()
{
printf(“Program start\n”);
MD1P.period_us(40);
MD2P.period_us(40);
MD3P.period_us(40);
MD4P.period_us(40);
while(1)
{
double position = ((double)wheel1.getPulses()/4098.0)*360.0;
// printf("%f\n", position);
PS3_Button();
PS3_Check();
if(up != 0)
{
mdp[1] = 1.0;
}else if(down != 0){
mdp[1] = -1.0;
}else{
mdp[1] = 0;
}
drivemotor();
}
}
void PS3_Button(void)
{
circle = sbdbt.getButtonState(PS3::maru);
triangle = sbdbt.getButtonState(PS3::sankaku);
square = sbdbt.getButtonState(PS3::sikaku);
cross = sbdbt.getButtonState(PS3::batu);
right = sbdbt.getButtonState(PS3::migi);
up = sbdbt.getButtonState(PS3::ue);
left = sbdbt.getButtonState(PS3::hidari);
down = sbdbt.getButtonState(PS3::sita);
LEFT1 = sbdbt.getButtonState(PS3::L1);
RIGHT1 = sbdbt.getButtonState(PS3::R1);
RIGHT2 = sbdbt.getButtonState(PS3::R2);
R_JOY_X = sbdbt.getRightJoystickXaxis();
R_JOY_Y = sbdbt.getRightJoystickYaxis();
L_JOY_X = sbdbt.getLeftJoystickXaxis();
L_JOY_Y = sbdbt.getLeftJoystickYaxis();
SELECT = sbdbt.getSELECTState();
START = sbdbt.getSTARTState();
}
void PS3_Check()
{
/*
if (START != 0)
pc.printf(“START\n\r”);
else if (SELECT != 0)
pc.printf(“SELECT\n\r”);
*/
if (circle != 0)
printf("circle\n\r");
else if (triangle != 0)
printf("triangle\n\r");
else if (square != 0)
printf("square\n\r");
else if (cross != 0)
printf("cross\n\r");
else if (right != 0)
printf("right\n\r");
else if (left != 0)
printf("left\n\r");
else if (up != 0)
printf("up\n\r");
else if (down != 0)
printf("down\n\r");
else if (RIGHT1 != 0)
printf("R1\n\r");
else if (LEFT1 != 0)
printf("L1\n\r");
else if (RIGHT2 != 0)
printf("R2\n\r");
else if (LEFT2 != 0)
printf("L2\n\r");
else if ((R_JOY_X != 0) || (R_JOY_Y != 0))
printf("R_X:%d / R_Y:%d\n\r", R_JOY_X, R_JOY_Y);
else if ((L_JOY_X != 0) || (L_JOY_Y != 0))
printf("L_X:%d / L_Y:%d\n\r", L_JOY_X, L_JOY_Y);
}
void drivemotor()
{
double md_max = 0.90;
int l;
for(l = 1; l <= 1; l++)
{
if(mdp[l] > md_max)
{
mdp[l] = md_max;
}
else if(mdp[l] < -1.0md_max)
{
mdp[l] = -1.0md_max;
}
if(mdp[l] >= 0)
{
mdd[l] = 1.0;
}
else if(mdp[l] < 0)
{
mdp[l] = fabs(mdp[l]);
mdd[l] = 0;
}
}
MD1P = mdp[1];
MD1D = mdd[1];
MD2P = mdp[2];
MD2D = mdd[2];
MD3P = mdp[3];
MD3D = mdd[3];
MD4P = mdp[4];
MD4D = mdd[4];
}