Diff: TestShell.cpp

Revision:

1:b2063ffa4927

Parent:

0:99a111b75cb4

--- a/TestShell.cpp	Tue Apr 26 16:09:17 2011 +0000
+++ b/TestShell.cpp	Thu Jan 11 07:47:26 2018 +0000
@@ -11,7 +11,6 @@
 
 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.
-
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
@@ -35,6 +34,22 @@
 #include "ps3.h"
 
 #include "mbed.h"
+/*
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+int joyL_x=0;
+int joyL_y=0;
+int angle_omni=0;
+*/
+//サブ関数宣言
+double d_r(double degree); //度数法→ラジアン
+double r_d(double radian); //ラジアン→度数法
+double max_min(double val,double max,double min); //最大最小値設定関数.math.hでいうmax関数とmin関数を統合した関数
+double odo_way(int mode); //オドメトリ用エンコーダーからとったpulseをmmに変換
+double mms_qpps(double millimeter_sec); //mmsからqpps(秒あたりのパルス数)に変換する関数
+void omni_move(double x_speed,double y_speed,double angle); 
+void odometry(); //オドメトリ関数
+
 
 
 void printf(const BD_ADDR* addr)
@@ -154,14 +169,36 @@
                     }
                     break;
                     default:
-                        printHex(data,len);
+                        printHex(data,len); 
                 }
              }
              if (t->_devClass == PS3_REMOTE)
              {   
+             /*
+                joyL_x=((ps3report*)(data+1))->LeftStickX;
+                joyL_y=((ps3report*)(data+1))->LeftStickY;
+                angle_omni=((ps3report*)(data+1))->RightStickY;
+                omni_move(joyL_x,joyL_y,angle_omni);
+                printf("\nx_global:%d\ty_global:%d\tangle:%d\t\n",joyL_x,joyL_y,angle_omni);
+                
+                if(joyL_x>200){
+                    led1=1;
+                    }
+                    else{
+                    led1=0;
+                        }
+                if(joyL_y>200){
+                    led2=1;
+                    }
+                    else{
+                    led2=0;
+                    
+                    }
+                 */
+                             
                 t->_count ++;
-                if (t->_count == 25) t->_count = 1;
-                ParsePs3Result((data + 1), sizeof(ps3report),t->_count);
+                //if (t->_count == 25) t->_count = 1;
+                //ParsePs3Result((data + 1), sizeof(ps3report),t->_count);
              }
              else {
                   printf("Not yet implemented \r\n");