Diff: DualShockMod.cpp

Revision:

0:d14e21c64226

Child:

1:8a67adccebd9

diff -r 000000000000 -r d14e21c64226 DualShockMod.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DualShockMod.cpp	Fri Sep 13 11:18:21 2019 +0000
@@ -0,0 +1,91 @@
+#include "DualShockMod.h"
+
+
+tDSParm hDS;
+int posX;
+int posY;
+static uint8_t recv_posdata, check = 0;
+static Serial* ds_serial;   //メイン
+//static float DimensionlessAndBacklash(int8_t dat);
+static uint8_t getsum(void *buf, size_t size);
+
+void ReStartDS(void) {
+    check = 0;
+//    printf("DSint1\r\n");
+    recv_posdata = (*ds_serial).getc();
+//    printf("DSint2\r\n");
+}
+
+uint8_t InitDS(Serial* f_serial) {
+    //huartDS = huart;
+    ds_serial = f_serial;
+    posX=0;     //externを実体化するための宣言
+    posY=0;
+
+    printf("SUB303 connecting check...\r\n");
+    ReStartDS();
+    printf("SUB303 connect successful...\r\n");
+    return 0;
+}
+//&getPOSdata
+void getPOSdata(void) {
+
+    static uint8_t dat[6] = {0};
+    recv_posdata = (*ds_serial).getc();
+    dat[check] = recv_posdata;
+    
+    //posX ++;  //debug
+    
+    
+    switch (check) {
+    case 0:
+        if (dat[0] & 0x80) {    // & は両方1なら1を返す
+            check++;
+        }
+        break;
+    case 5:
+        if (dat[5] == (getsum(dat, 5) & 0x7f)) {      //ここは0~5までの合計の意味
+            if(dat[0] & 0b00000010){ //Xが負であるビット            
+                posX = (int)((dat[1]<<7) + dat[2])*-1;
+            }
+            else{
+                posX = ((dat[1]<<7) + dat[2]);
+            }
+            if(dat[0] & 0b00000001){ //Yが負であるビット            
+                posY = (int)((dat[3]<<7) + dat[4])*-1;
+            }
+            else{
+                posY = (dat[3]<<7) + dat[4];
+            }
+        }
+        dat[0] = 0;
+        check = 0;
+        break;
+    default:
+        check++;
+        break;
+    }
+}
+/*
+static float DimensionlessAndBacklash(int8_t dat) {
+    float val;
+
+    if ((dat < BACKLASH) && (dat > -BACKLASH)) {
+        dat = 0;
+    } else {
+        dat += (dat > 0) ? (-BACKLASH) : (BACKLASH);
+    }
+    val = (float) dat / (float) (128 - BACKLASH);
+    return val;
+}
+*/
+static uint8_t getsum(void *buf, size_t size) {
+    const uint8_t *p = (uint8_t*) buf;
+    uint8_t ret = 0;
+
+    for (; size; size--) {
+        ret += *p++;
+    }
+    return ret;
+}
+