ds

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers DualShock.cpp Source File

DualShock.cpp

00001 #include "DualShock.h"
00002 
00003 tDSParm hDS;
00004 static uint8_t recv_data, check = 0;
00005 static Serial* ds_serial;
00006 static float DimensionlessAndBacklash(int8_t dat);
00007 static uint8_t getsum(void *buf, size_t size);
00008 
00009 void ReStartDS(void) {
00010     check = 0;
00011     recv_data = (*ds_serial).getc();
00012 }
00013 
00014 uint8_t InitDS(Serial* f_serial) {
00015 
00016     hDS.BUTTON.ButtonData = 0;
00017     hDS.ANALOG.LX = 0;
00018     hDS.ANALOG.LY = 0;
00019     hDS.ANALOG.RX = 0;
00020     hDS.ANALOG.RY = 0;
00021     //huartDS = huart;
00022     ds_serial = f_serial;
00023 
00024     printf("DualShock connecting check...\r\n");
00025     ReStartDS();
00026     printf("DualShock connect successful...\r\n");
00027     return 0;
00028 }
00029 //&getDSdata
00030 void getDSdata(void) {
00031     static uint8_t dat[8] = {0};
00032 
00033     recv_data = (*ds_serial).getc();
00034     dat[check] = recv_data;
00035     switch (check) {
00036     case 0:
00037         if (dat[0] == 0x80) {
00038             check++;
00039         }
00040         break;
00041     case 7:
00042         if (dat[7] == (getsum(dat, 7) & 0x7f)) {
00043             hDS.BUTTON.ButtonHighData = dat[1];
00044             hDS.BUTTON.ButtonLowData = dat[2];
00045             hDS.ANALOG.LX = DimensionlessAndBacklash(dat[3]);
00046             hDS.ANALOG.LY = DimensionlessAndBacklash(dat[4]);
00047             hDS.ANALOG.RX = DimensionlessAndBacklash(dat[5]);
00048             hDS.ANALOG.RY = DimensionlessAndBacklash(dat[6]);
00049         }
00050         dat[0] = 0;
00051         check = 0;
00052         break;
00053     default:
00054         check++;
00055         break;
00056     }
00057 }
00058 
00059 static float DimensionlessAndBacklash(int8_t dat) {
00060     float val;
00061 
00062     if ((dat < BACKLASH) && (dat > -BACKLASH)) {
00063         dat = 0;
00064     } else {
00065         dat += (dat > 0) ? (-BACKLASH) : (BACKLASH);
00066     }
00067     val = (float) dat / (float) (128 - BACKLASH);
00068     return val;
00069 }
00070 
00071 static uint8_t getsum(void *buf, size_t size) {
00072     const uint8_t *p = (uint8_t*) buf;
00073     uint8_t ret = 0;
00074 
00075     for (; size; size--) {
00076         ret += *p++;
00077     }
00078     return ret;
00079 }