DualShockMod.cpp

00001 #include "DualShockMod.h"
00002 
00003 
00004 tDSParm hDS;
00005 int posX;
00006 int posY;
00007 static uint8_t recv_posdata, check = 0;
00008 static Serial* ds_serial;   //メイン
00009 //static float DimensionlessAndBacklash(int8_t dat);
00010 static uint8_t getsum(void *buf, size_t size);
00011 
00012 void ReStartDS(void) {
00013     check = 0;
00014 //    printf("DSint1\r\n");
00015     recv_posdata = (*ds_serial).getc();
00016 //    printf("DSint2\r\n");
00017 }
00018 
00019 uint8_t InitDS(Serial* f_serial) {
00020     //huartDS = huart;
00021     ds_serial = f_serial;
00022     posX=0;     //externを実体化するための宣言
00023     posY=0;
00024 
00025     printf("SUB303 connecting check...\r\n");
00026     ReStartDS();
00027     printf("SUB303 connect successful...\r\n");
00028     return 0;
00029 }
00030 //&getPOSdata
00031 void getPOSdata(void) {
00032 
00033     static uint8_t dat[6] = {0};
00034     recv_posdata = (*ds_serial).getc();
00035     dat[check] = recv_posdata;
00036     
00037     //posX ++;  //debug
00038     
00039     
00040     switch (check) {
00041     case 0:
00042         if (dat[0] & 0x80) {    // & は両方1なら1を返す
00043             check++;
00044         }
00045         break;
00046     case 5:
00047         if (dat[5] == (getsum(dat, 5) & 0x7f)) {      //ここは0~5までの合計の意味
00048             if(dat[0] & 0b00000010){ //Xが負であるビット            
00049                 posX = (int)((dat[1]<<7) + dat[2])*-1;
00050             }
00051             else{
00052                 posX = ((dat[1]<<7) + dat[2]);
00053             }
00054             if(dat[0] & 0b00000001){ //Yが負であるビット            
00055                 posY = (int)((dat[3]<<7) + dat[4])*-1;
00056             }
00057             else{
00058                 posY = (dat[3]<<7) + dat[4];
00059             }
00060         }
00061         dat[0] = 0;
00062         check = 0;
00063         break;
00064     default:
00065         check++;
00066         break;
00067     }
00068 }
00069 /*
00070 static float DimensionlessAndBacklash(int8_t dat) {
00071     float val;
00072 
00073     if ((dat < BACKLASH) && (dat > -BACKLASH)) {
00074         dat = 0;
00075     } else {
00076         dat += (dat > 0) ? (-BACKLASH) : (BACKLASH);
00077     }
00078     val = (float) dat / (float) (128 - BACKLASH);
00079     return val;
00080 }
00081 */
00082 static uint8_t getsum(void *buf, size_t size) {
00083     const uint8_t *p = (uint8_t*) buf;
00084     uint8_t ret = 0;
00085 
00086     for (; size; size--) {
00087         ret += *p++;
00088     }
00089     return ret;
00090 }
00091