Revision 0:4289fbe66d45, committed 2020-06-21

Comitter:

Kize

Date:

Sun Jun 21 01:48:47 2020 +0000

Commit message:

new commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dualshock3.cpp	Sun Jun 21 01:48:47 2020 +0000
@@ -0,0 +1,444 @@
+#include "dualshock3.h"
+
+Dualshock3::Dualshock3(PinName tx, PinName rx)
+:controller(tx, rx), baudrate(DEFAULT_BAUDRATE)
+{
+    button_state = 0;
+    button_risen = 0;
+    previous_button_state = button_state;
+    data[3] = 0x40;
+    data[4] = 0x40;
+    data[5] = 0x40;
+    data[6] = 0x40;
+    printf("dualshock3 is ready\r\n");
+}
+
+void Dualshock3::set_baudrate(int baud)
+{
+    baudrate = baud;
+}
+
+void Dualshock3::initialize(void)
+{
+    controller.baud(baudrate);
+    controller.attach(callback(this, &Dualshock3::read), Serial::RxIrq);
+}
+
+void Dualshock3::clear(void)
+{
+    button_state = 0;
+    button_risen = 0;
+}
+
+void Dualshock3::read(void)
+{
+    __disable_irq();
+    data[0] = controller.getc();
+    if(data[0]==0x80){
+        for(int i=1;i<DATA_LENGTH;i++){
+            data[i] = controller.getc();
+        }
+        button_state = (data[1]<<DATA_LENGTH) + data[2];
+        //今該当ボタンのビットが1で、前回は0なら、risenを1にする
+        if((button_state & UP) && !(previous_button_state & UP)){
+            button_risen |= UP;
+        }
+        if((button_state & DOWN) && !(previous_button_state & DOWN)){
+            button_risen |= DOWN;
+        }
+        if((button_state & LEFT) && !(previous_button_state & LEFT)){
+            button_risen |= LEFT;
+        }
+        if((button_state & RIGHT) && !(previous_button_state & RIGHT)){
+            button_risen |= RIGHT;
+        }
+        if((button_state & TRIANGLE) && !(previous_button_state & TRIANGLE)){
+            button_risen |= TRIANGLE;
+        }
+        if((button_state & CROSS) && !(previous_button_state & CROSS)){
+            button_risen |= CROSS;
+        }
+        if((button_state & CIRCLE) && !(previous_button_state & CIRCLE)){
+            button_risen |= CIRCLE;
+        }
+        if((button_state & RECTANGLE) && !(previous_button_state & RECTANGLE)){
+            button_risen |= RECTANGLE;
+        }
+        if((button_state & L1) && !(previous_button_state & L1)){
+            button_risen |= L1;
+        }
+        if((button_state & L2) && !(previous_button_state & L2)){
+            button_risen |= L2;
+        }
+        if((button_state & R1) && !(previous_button_state & R1)){
+            button_risen |= R1;
+        }
+        if((button_state & R2) && !(previous_button_state & R2)){
+            button_risen |= R2;
+        }
+        if((button_state & START) && !(previous_button_state & START)){
+            button_risen |= START;
+        }
+        if((button_state & SELECT) && !(previous_button_state & SELECT)){
+            button_risen |= SELECT;
+        }
+        previous_button_state = button_state;
+    }
+    __enable_irq();
+}
+
+bool Dualshock3::circle_is_pushed(void)
+{
+    if(button_state & CIRCLE){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::cross_is_pushed(void)
+{
+    if(button_state & CROSS){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::rectangle_is_pushed(void)
+{
+    if(button_state & RECTANGLE){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::triangle_is_pushed(void)
+{
+    if(button_state & TRIANGLE){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::up_is_pushed(void)
+{
+    if(button_state & UP){
+        if(button_state & DOWN){
+            return 0;
+        }else{
+            return true;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::down_is_pushed(void)
+{
+    if(button_state & DOWN){
+        if(button_state & UP){
+            return 0;
+        }else{
+            return true;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::left_is_pushed(void)
+{
+    if(button_state & LEFT){
+        if(button_state & RIGHT){
+            return 0;
+        }else{
+            return true;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::right_is_pushed(void)
+{
+    if(button_state & RIGHT){
+        if(button_state & LEFT){
+            return 0;
+        }else{
+            return true;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::l1_is_pushed(void)
+{
+    if(button_state & L1){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::l2_is_pushed(void)
+{
+    if(button_state & L2){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::r1_is_pushed(void)
+{
+    if(button_state & R1){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::r2_is_pushed(void)
+{
+    if(button_state & R2){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::start_is_pushed(void)
+{
+    if((button_state & START) == START){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::select_is_pushed(void)
+{
+    if((button_state & SELECT) == SELECT){
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+float Dualshock3::get_left_stick_x(void)
+{
+    return (float)(data[3] - CENTER_STICK_VAL) / CENTER_STICK_VAL;
+}
+
+float Dualshock3::get_left_stick_y(void)
+{
+    return -(float)(data[4] - CENTER_STICK_VAL) / CENTER_STICK_VAL;
+}
+
+float Dualshock3::get_right_stick_x(void)
+{
+    return (float)(data[5] - CENTER_STICK_VAL) / CENTER_STICK_VAL;
+}
+
+float Dualshock3::get_right_stick_y(void)
+{
+    return -(float)(data[6] - CENTER_STICK_VAL) / CENTER_STICK_VAL;
+}
+
+bool Dualshock3::circle_has_been_pushed(void)
+{
+    if(button_risen & CIRCLE){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::cross_has_been_pushed(void)
+{
+    if(button_risen & CROSS){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::rectangle_has_been_pushed(void)
+{
+    if(button_risen & RECTANGLE){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::triangle_has_been_pushed(void)
+{
+    if(button_risen & TRIANGLE){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::up_has_been_pushed(void)
+{
+    if(button_risen & UP){
+        if(up_is_pushed()){
+            button_risen = 0;
+            return true;
+        }else{
+            return 0;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::down_has_been_pushed(void)
+{
+    if(button_risen & DOWN){
+        if(down_is_pushed()){
+            button_risen = 0;
+            return true;
+        }else{
+            return 0;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::left_has_been_pushed(void)
+{
+    if(button_risen & LEFT){
+        if(left_is_pushed()){
+            button_risen = 0;
+            return true;
+        }else{
+            return 0;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::right_has_been_pushed(void)
+{
+    if(button_risen & RIGHT){
+        if(right_is_pushed()){
+            button_risen = 0;
+            return true;
+        }else{
+            return 0;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::l1_has_been_pushed(void)
+{
+    if(button_risen & L1){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::l2_has_been_pushed(void)
+{
+    if(button_risen & L2){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::r1_has_been_pushed(void)
+{
+    if(button_risen & R1){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::r2_has_been_pushed(void)
+{
+    if(button_risen & R2){
+        button_risen = 0;
+        return true;
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::start_has_been_pushed(void)
+{
+    if((button_risen & START) == START){
+        if(start_is_pushed()){
+            button_risen = 0;
+            return true;
+        }else{
+            return 0;
+        }
+    }else{
+        return 0;
+    }
+}
+
+bool Dualshock3::select_has_been_pushed(void)
+{
+    if((button_risen & SELECT) == SELECT){
+        if(select_is_pushed()){
+            button_risen = 0;
+            return true;
+        }else{
+            return 0;
+        }
+    }else{
+        return 0;
+    }
+}
+
+void Dualshock3::get_all_buttons_info(int buttons_info[14]){
+    /*
+    for(int i=0;i<14;i++){
+        buttons_info[i] = 0;
+    }
+    */
+
+    buttons_info[0]  = select_is_pushed();
+    buttons_info[1]  = start_is_pushed();
+    buttons_info[2]  = up_is_pushed();
+    buttons_info[3]  = right_is_pushed();
+    buttons_info[4]  = down_is_pushed();
+    buttons_info[5]  = left_is_pushed();
+    buttons_info[6]  = l2_is_pushed();
+    buttons_info[7]  = r2_is_pushed();
+    buttons_info[8]  = l1_is_pushed();
+    buttons_info[9]  = r1_is_pushed();
+    buttons_info[10] = triangle_is_pushed();
+    buttons_info[11] = circle_is_pushed();
+    buttons_info[12] = cross_is_pushed();
+    buttons_info[13] = rectangle_is_pushed();
+}
+
+void Dualshock3::get_all_sticks_info(float sticks_info[4]){
+    sticks_info[0] = get_left_stick_x();
+    sticks_info[1] = get_left_stick_y();
+    sticks_info[2] = get_right_stick_x();
+    sticks_info[3] = get_right_stick_y();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dualshock3.h	Sun Jun 21 01:48:47 2020 +0000
@@ -0,0 +1,79 @@
+#ifndef __DUALSHOCK3
+#define __DUALSHOCK3
+
+#include "mbed.h"
+
+class Dualshock3
+{
+public:
+    Dualshock3(PinName, PinName);
+
+    void set_baudrate(int);
+    void initialize(void);
+    bool circle_is_pushed(void);
+    bool cross_is_pushed(void);
+    bool rectangle_is_pushed(void);
+    bool triangle_is_pushed(void);
+    bool up_is_pushed(void);
+    bool down_is_pushed(void);
+    bool left_is_pushed(void);
+    bool right_is_pushed(void);
+    bool start_is_pushed(void);
+    bool select_is_pushed(void);
+    bool r1_is_pushed(void);
+    bool l1_is_pushed(void);
+    bool r2_is_pushed(void);
+    bool l2_is_pushed(void);
+    float get_left_stick_x(void);
+    float get_left_stick_y(void);
+    float get_right_stick_x(void);
+    float get_right_stick_y(void);
+    bool circle_has_been_pushed(void);
+    bool cross_has_been_pushed(void);
+    bool rectangle_has_been_pushed(void);
+    bool triangle_has_been_pushed(void);
+    bool up_has_been_pushed(void);
+    bool down_has_been_pushed(void);
+    bool left_has_been_pushed(void);
+    bool right_has_been_pushed(void);
+    bool start_has_been_pushed(void);
+    bool select_has_been_pushed(void);
+    bool r1_has_been_pushed(void);
+    bool l1_has_been_pushed(void);
+    bool r2_has_been_pushed(void);
+    bool l2_has_been_pushed(void);
+    void clear(void);
+
+    void get_all_buttons_info(int buttons_info[14]);
+    void get_all_sticks_info(float sticks_info[4]);
+
+private:
+    static const int DEFAULT_BAUDRATE = 115200;
+    static const int DATA_LENGTH = 8;
+    static const float CENTER_STICK_VAL = 64;
+    static const int UP        = 0x2;//0b 0000 0000 0000 0001
+    static const int DOWN      = 0x0002;//0b 0000 0000 0000 0010
+    static const int RIGHT     = 0x0004;//0b 0000 0000 0000 0100
+    static const int LEFT      = 0x0008;//0b 0000 0000 0000 1000
+    static const int TRIANGLE  = 0x0010;//0b 0000 0000 0001 0000
+    static const int CROSS     = 0x0020;//0b 0000 0000 0010 0000
+    static const int CIRCLE    = 0x0040;//0b 0000 0000 0100 0000
+    static const int RECTANGLE = 0x0100;//0b 0000 0001 0000 0000
+    static const int L1        = 0x0200;//0b 0000 0010 0000 0000
+    static const int L2        = 0x0400;//0b 0000 0100 0000 0000
+    static const int R1        = 0x0800;//0b 0000 1000 0000 0000
+    static const int R2        = 0x1000;//0b 0001 0000 0000 0000
+    static const int START     = 0x0003;//0b 0000 0000 0000 0011
+    static const int SELECT    = 0x000C;//0b 0000 0000 0000 1100
+
+    Serial controller;
+    int baudrate;
+    //char data[DATA_LENGTH];
+    int data[DATA_LENGTH];
+    int button_state;
+    int button_risen;
+    int previous_button_state;
+
+    void read(void);
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jun 21 01:48:47 2020 +0000
@@ -0,0 +1,105 @@
+#include "mbedserial.h"
+#include "dualshock3.h"
+
+Dualshock3 controller(D1,D0);
+Serial pc(USBTX,USBRX,115200);
+Mbedserial Ms(pc);
+
+/*
+void CallBack_float();
+void CallBack_int();
+void CallBack_char();
+*/
+
+int main(){
+  //受信コールバック関数の設定
+/*
+  Ms.float_attach(CallBack_float);
+  Ms.int_attach(CallBack_int);
+  Ms.char_attach(CallBack_char);
+*/
+
+  //ボーレート(通信速度)を2400bpsに設定
+  controller.set_baudrate(2400);
+  //dualshock3との通信開始
+  controller.initialize();
+
+//  int data[2] = {70,70};
+  int buttons_info[14] = {};
+  float sticks_info[4] = {};
+
+  while(1){
+    controller.get_all_buttons_info(buttons_info);
+    controller.get_all_sticks_info(sticks_info);
+/*
+    pc.printf("buttons:");
+    for(int i=0;i<14;i++){
+      pc.printf("%d ",buttons_info[i]);
+    }
+    pc.printf("\r\n");
+    pc.printf("sticks:");
+    for(int i=0;i<4;i++){
+      pc.printf("%f_",sticks_info[i]);
+    }
+    pc.printf("\r\n");
+*/
+/*    
+    printf("L_x = %.2f  ", controller.get_left_stick_x());
+    printf("L_y = %.2f  ", controller.get_left_stick_y());
+    printf("R_x = %.2f  ", controller.get_right_stick_x());
+    printf("R_y = %.2f\r\n", controller.get_right_stick_y());
+*/
+    Ms.int_write(buttons_info,14);
+    wait(0.5);
+    Ms.float_write(sticks_info,4);
+    wait(0.5);
+
+ //   Ms.int_write(data,2);
+/*
+    if(data[0]<73)
+      data[0]++;
+    else
+      data[0]-=3;
+    if(data[1]>70)
+      data[1]--;
+    else    
+      data[1]+=3;
+ */
+      
+    //ループの最後に呼ぶこと
+    controller.clear();
+    wait(0.1);
+  }
+}
+
+/*
+void CallBack_float(){
+  //受信
+  //配列サイズを取得
+  int size = Ms.floatarraysize;
+  //データを取得
+  float *f = Ms.getfloat;
+  //送信
+  Ms.float_write(f,size);
+}
+
+void CallBack_int(){
+  //受信
+  //配列サイズを取得
+  int size = Ms.intarraysize;
+  //データを取得
+  int *i = Ms.getint;
+  //送信
+  Ms.int_write(i,size);
+}
+
+void CallBack_char(){
+  //受信
+  //配列サイズを取得
+  int size = Ms.chararraysize;
+  //データを取得
+  char *c = Ms.getchar;
+  //送信
+  Ms.char_write(c,size);
+}
+*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Jun 21 01:48:47 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbedserial.cpp	Sun Jun 21 01:48:47 2020 +0000
@@ -0,0 +1,163 @@
+// mbedserial.cpp
+// serial communication : mbed <-> ROS
+
+#include "mbedserial.h"
+
+/* init */
+void _nullfunc(){ ; };
+
+Mbedserial::Mbedserial(Serial &pc) : rospc(pc)
+{
+    msg_buf = new char[256];
+    bufsize = 0;
+    endmsg = '\n';
+    floatarraysize = 0;
+    intarraysize = 0;
+    chararraysize = 0;
+    for(int i=0;i<3;i++){
+        pfunccb[i] = _nullfunc;
+    }
+    rospc.attach(callback(this,&Mbedserial::rcv_callback),Serial::RxIrq);
+}
+
+/* receive */
+void Mbedserial::rcv_callback()
+{
+    msg_buf[bufsize] = rospc.getc();
+    if(msg_buf[bufsize] == endmsg){
+        switch(msg_buf[0]){
+            case 'f':
+                floatarraysize = *(int *)(&msg_buf[1]);
+                //memcpy(&floatarraysize,&msg_buf[1],4);
+                if(bufsize == floatarraysize * 4 + 5){
+                    for(int i=0;i<floatarraysize;i++){
+                        getfloat[i] = *(float *)(&msg_buf[i*4+5]);
+                        //memcpy(&getfloat[i],&msg_buf[i*4+5]);
+                    }
+                    pfunccb[0]();
+                }
+                break;
+            case 'i':
+                intarraysize = *(int *)(&msg_buf[1]);
+                //memcpy(&intarraysize, &msg_buf[1], 4);
+                if (bufsize == intarraysize * 4 + 5)
+                {
+                    for (int i = 0; i < intarraysize; i++)
+                    {
+                        getint[i] = *(int *)(&msg_buf[i * 4 + 5]);
+                        //memcpy(&getint[i], &msg_buf[i * 4 + 5], 4);
+                    }
+                    pfunccb[1]();
+                }
+                break;
+            case 'c':
+                chararraysize = *(int *)(&msg_buf[1]);
+                //memcpy(&chararraysize, &msg_buf[1], 4);
+                if (bufsize == chararraysize + 5)
+                {
+                    memcpy(&getchar[0], &msg_buf[5], chararraysize);
+                    pfunccb[2]();
+                }
+                break;
+        }
+        delete[] msg_buf;
+        bufsize = 0;
+        msg_buf = new char[256];
+    }else{
+        bufsize++;
+    }
+}
+
+/* send */
+int pub_wait_time = 8; //ms
+
+void Mbedserial::float_write(float array[], int arraysize)
+{
+    // send data type
+    char msg_type = 'f';
+    rospc.putc(msg_type);
+
+    // send array size
+    char arraysize_c[4];
+    *(int *)arraysize_c = arraysize;
+    //memcpy(arraysize_c, &arraysize, 4);
+    for (int i = 0; i < 4; i++)
+    {
+        rospc.putc(arraysize_c[i]);
+    }
+
+    // send float data
+    char array_c[4];
+    for (int i = 0; i < arraysize; i++)
+    {
+        *(float *)array_c = array[i];
+        //memcpy(array_c, &array[i], 4);
+        for (int j = 0; j < 4; j++)
+        {
+            rospc.putc(array_c[j]);
+        }
+    }
+
+    // send end message
+    rospc.putc(endmsg);
+    //wait_ms(pub_wait_time);
+}
+
+void Mbedserial::int_write(int *array, int arraysize)
+{
+    // send data type
+    char msg_type = 'i';
+    rospc.putc(msg_type);
+
+    // send array size
+    char arraysize_c[4];
+   
+    *(int *)arraysize_c = arraysize;
+    //memcpy(arraysize_c, &arraysize, 4);
+    for (int i = 0; i < 4; i++)
+    {
+        rospc.putc(arraysize_c[i]);
+    }
+
+    // send int data
+    char array_c[4];
+    for (int i = 0; i < arraysize; i++)
+    {
+        *(int *)array_c = array[i];
+        //memcpy(array_c, &array[i], 4);
+        for (int j = 0; j < 4; j++)
+        {
+            rospc.putc(array_c[j]);
+        }
+    }
+
+    // send end message
+    rospc.putc(endmsg);
+    //wait_ms(pub_wait_time);
+}
+
+void Mbedserial::char_write(char *array, int arraysize)
+{
+    // send data type
+    char msg_type = 'c';
+    rospc.putc(msg_type);
+
+    // send array size
+    char arraysize_c[4];
+    *(int *)arraysize_c = arraysize;
+    //memcpy(arraysize_c, &arraysize, 4);
+    for (int i = 0; i < 4; i++)
+    {
+        rospc.putc(arraysize_c[i]);
+    }
+
+    // send char data
+    for (int i = 0; i < arraysize; i++)
+    {
+        rospc.putc(array[i]);
+    }
+
+    // send end message
+    rospc.putc(endmsg);
+    //wait_ms(pub_wait_time);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbedserial.h	Sun Jun 21 01:48:47 2020 +0000
@@ -0,0 +1,44 @@
+// mbedserial.h
+// serial communication : mbed <-> ROS
+
+#ifndef _MBEDSERIAL_
+#define _MBEDSERIAL_
+
+#include <mbed.h>
+#include <stdio.h>
+#include <string.h>
+
+// mbedserial class
+class Mbedserial
+{
+    private:
+        char *msg_buf;
+        int bufsize;
+        char endmsg;
+        Serial& rospc;
+        void rcv_callback();
+        void (*pfunccb[3])();
+
+    public:
+        float getfloat[32];
+        int getint[32];
+        char getchar[256];
+        int floatarraysize;
+        int intarraysize;
+        int chararraysize;
+        Mbedserial(Serial&);
+        void float_write(float *array,int arraysize);
+        void int_write(int *array,int arraysize);
+        void char_write(char *array,int arraysize);
+        void float_attach(void (*pfunc)()){
+            pfunccb[0] = pfunc;
+        };
+        void int_attach(void (*pfunc)()){
+            pfunccb[1] = pfunc;
+        };
+        void char_attach(void (*pfunc)()){
+            pfunccb[2] = pfunc;
+        };
+};
+
+#endif
\ No newline at end of file