Revision 1:b2063ffa4927, committed 2018-01-11

Comitter:

e5115026

Date:

Thu Jan 11 07:47:26 2018 +0000

Parent:

0:99a111b75cb4

Commit message:

???????

Changed in this revision

diff -r 99a111b75cb4 -r b2063ffa4927 AutoEvents.cpp
--- a/AutoEvents.cpp	Tue Apr 26 16:09:17 2011 +0000
+++ b/AutoEvents.cpp	Thu Jan 11 07:47:26 2018 +0000
@@ -258,8 +258,8 @@
 {
     printf("LoadDevice %d %02X:%02X:%02X\r\n",device,interfaceDesc->bInterfaceClass,interfaceDesc->bInterfaceSubClass,interfaceDesc->bInterfaceProtocol);
     char s[128];
-    u8 my_mac[6] = {0x00, 0x02, 0x72, 0xAD, 0xF3, 0x5B}; // mac address of my Bluetooth device
-    
+    u8 my_mac[6] = {0x00, 0x1B, 0xDC, 0x0C, 0x9D, 0x41}; // mac address of my Bluetooth device
+    //00:1B:DC:0C:9D:41
     u8 buf2[6];
     
     buf2[0] = 0x00;

diff -r 99a111b75cb4 -r b2063ffa4927 FATFileSystem.lib
--- a/FATFileSystem.lib	Tue Apr 26 16:09:17 2011 +0000
+++ b/FATFileSystem.lib	Thu Jan 11 07:47:26 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_unsupported/code/fatfilesystem/
\ No newline at end of file
+http://mbed.org/users/mbed_unsupported/code/fatfilesystem/#333d6e93e58f

diff -r 99a111b75cb4 -r b2063ffa4927 PS3_BlueUSB.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PS3_BlueUSB.lib	Thu Jan 11 07:47:26 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/BartJanssens/code/PS3_BlueUSB/#99a111b75cb4

diff -r 99a111b75cb4 -r b2063ffa4927 Ps3USB.cpp
--- a/Ps3USB.cpp	Tue Apr 26 16:09:17 2011 +0000
+++ b/Ps3USB.cpp	Thu Jan 11 07:47:26 2018 +0000
@@ -36,10 +36,61 @@
 #define AUTOEVT(_class,_subclass,_protocol) (((_class) << 16) | ((_subclass) << 8) | _protocol)
 #define PS3EVT AUTOEVT(CLASS_HID,0,0)
 #define byteswap(x) ((x >> 8) | (x << 8))
+#define device_baud 115200      //上のマイコンにするバンド数
+#define send_device_rate 0.03   //割り込みレート
+
 
 u8 ps3_data[48];
+Serial order_device(p28,p27);//上のマイコンに反応させるための初期化宣言
 
 
+//*************追加項目ピン設定
+PwmOut led1(LED1);
+PwmOut led2(LED2);
+PwmOut led3(LED3);
+PwmOut led4(LED4);
+int joyL_x  =0;
+int joyL_y  =0;
+int joyR_x  =0;
+int joyR_y  =0;
+int L1      =0;
+int R1      =0;
+int up      =0; 
+int down    =0;
+int right   =0;
+int left    =0;
+int Xmove       =0;
+int Ymove       =0;
+int angle_omni  =0;
+
+int speed_mode    =1;
+
+void omni_move(double x_speed,double y_speed,double angle); 
+char Buf[8];
+
+
+//**************
+
+//以下項目上への指令を送るためのBuf
+/*
+void send_device(){
+    char Buf[8];
+    Buf[0] = up;
+    Buf[1] = down;
+    Buf[2] = right;
+    Buf[3] = left;
+    Buf[4] = 0;
+    Buf[5] = 0;
+    order_device.putc(Buf[0]);
+    order_device.putc(Buf[1]);
+    order_device.putc(Buf[2]);
+    order_device.putc(Buf[3]);
+    order_device.putc(Buf[4]);
+    order_device.putc(Buf[5]);
+}
+*/
+//*********まで
+
 Ps3USB::Ps3USB(int device, int configuration, int interfaceNumber)
 {
         printf("Creating new sixaxis \r\n");
@@ -211,9 +262,18 @@
         wait_ms(4);
 
     }
+    /*
+    void Connect(){
+        order_device.baud(device_baud);//上のマイコンとのbaud指定
+        tick_device.attach(&send_device,send_device_rate);//割り込み
+        }
+    */
+    
+    
     
     int ParsePs3Result(const u8* data,int len,int count)
-    {
+    {   
+        order_device.baud(device_baud);
         ps3report* _ps3report = (ps3report*)data;    
         if (count == 24) printf("LSX LSY RSX RSY UPA RPA DPA RPA L2  R2  L1  R1  TRI CIR CRO SQU  ACX  ACY  ACZ  GYZ  \r\n");
         printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
@@ -238,6 +298,59 @@
                             (_ps3report->AccelZ),
                             (_ps3report->GyroZ));
         //printfBytes("data",data,len);
-    }
+        joyL_x  =_ps3report->LeftStickX;
+        joyL_y  =_ps3report->LeftStickY;
+        joyR_x  =_ps3report->RightStickY;
+        joyR_y  =_ps3report->RightStickX;
+        L1      =_ps3report->PressureL1;
+        R1      =_ps3report->PressureR1;
+        up      =_ps3report->PressureUp;
+        down    =_ps3report->PressureDown;
+        right   =_ps3report->PressureRight;
+        left    =_ps3report->PressureLeft;
+        
+        
+        //デットバンドの設定
+        if(120<=joyL_x&&joyL_x<=129)
+        {
+            joyL_x=126;
+            }
+        if(120<=joyL_y&&joyL_y<=129)
+        {
+            joyL_y=126;
+            }
+        if(120<=joyR_x&&joyR_x<=129)
+        {
+            joyR_x=126;
+            }
+        if(120<=joyR_y&&joyR_y<=129)
+        {
+            joyR_y=126;
+            }
+        
+        
+        //低速モードと高速モード設定
+        if(L1>=100){
+            speed_mode=2;
+            }
+        else{
+            speed_mode=20;
+            }
+        
+        
+        order_device.printf("%d,%d,%d,%d\n", up, down, right, left);
+        
+        
+        Xmove       =(joyL_x-126)*speed_mode;
+        Ymove       =(joyL_y-126)*speed_mode;
+        angle_omni  =(joyR_y-126)*speed_mode;
+        
+        omni_move(angle_omni,Ymove,Xmove);
+        led1=joyL_x/255.0;
+        led2=joyL_y/255.0;
+        led3=joyR_y/255.0;
+        
+        printf("joyL_x:%d joyL_y:%d angle_omni:%d \r\n" , Xmove, Ymove, angle_omni);
+                    } 
 
     

diff -r 99a111b75cb4 -r b2063ffa4927 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Jan 11 07:47:26 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/kouika748/code/QEI/#0f7cddbaf065

diff -r 99a111b75cb4 -r b2063ffa4927 RoboClaw_mine_.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw_mine_.lib	Thu Jan 11 07:47:26 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/2018_Project-R/code/RoboClaw_mine_/#0d30729e8885

diff -r 99a111b75cb4 -r b2063ffa4927 TestShell.cpp
--- 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");

diff -r 99a111b75cb4 -r b2063ffa4927 hci.cpp
--- a/hci.cpp	Tue Apr 26 16:09:17 2011 +0000
+++ b/hci.cpp	Thu Jan 11 07:47:26 2018 +0000
@@ -105,10 +105,13 @@
 
 int HCI::WriteScanEnable()
 {
-   
-    u8 buf[2];
-    buf[0] = 0x03;
-    buf[1] = 0x01;    
+    //ライブラリをそのまま使うと使えないため修正項目です．
+    //u8 buf[2];
+    //buf[0] = 0x03;
+    //buf[1] = 0x01;    
+    
+    u8 buf[1];
+    buf[0]=0x03;
     
     SendCmd(HCI_OP_WRITE_SCAN_ENABLE,buf,sizeof(buf));
     return 0;
@@ -116,10 +119,16 @@
 
 int HCI::AcceptConnection(const BD_ADDR* addr)
 {
-    u8 buf[6+4];
+    //ライブラリをそのまま使うと使えないため修正項目です．
+    //u8 buf[6+4];
+    //memset(buf,0,sizeof(buf));
+    //memcpy(buf,addr,6);
+    //buf[7] = 0;
+    
+    u8 buf[6+1];
     memset(buf,0,sizeof(buf));
     memcpy(buf,addr,6);
-    buf[7] = 0;      
+    buf[0]=0;     
     
     SendCmd(HCI_OP_ACCEPT_CONN_REQ,buf,sizeof(buf));
     return 0;

diff -r 99a111b75cb4 -r b2063ffa4927 main.cpp
--- a/main.cpp	Tue Apr 26 16:09:17 2011 +0000
+++ b/main.cpp	Thu Jan 11 07:47:26 2018 +0000
@@ -111,11 +111,16 @@
     return c;
 }
 
+void setup_omni();
+
 void TestShell();
 
 int main()
 {
-    pc.baud(9600);
+    setup_omni();
+    wait(4.0);
+    
+    pc.baud(115200);
     printf("BlueUSB\nNow get a bunch of usb or bluetooth things and plug them in\r\n");
     TestShell();
 }

diff -r 99a111b75cb4 -r b2063ffa4927 omni.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/omni.cpp	Thu Jan 11 07:47:26 2018 +0000
@@ -0,0 +1,166 @@
+
+#include "mbed.h"
+#include "RoboClaw.h"
+#include "QEI.h"
+
+//位置PID
+#define Kp 100
+#define Ki 7
+#define Kd 0
+
+//回転角度PID
+#define a_Kp 100
+#define a_Ki 7
+#define a_Kd 0
+
+//オドメトリ用輪(mm
+#define omni1 40.9401
+#define omni2 41.0987
+#define omni3 40.6526
+
+#define radius 101.6 //駆動輪直径(mm
+#define gaisetuen 121.24355652982 //オドメトリ用車輪の外接円
+#define Pi 3.141592 //円周率
+#define dt 0.001 //動作周期時間(s)
+
+#define resolution 200 //オドメトリエンコーダーの分解能
+#define en_bai 2 //エンコーダーの倍する変数
+
+//縦：X 横:Y
+
+Ticker tick_odo; //オドメトリ関数を時間割り込みさせる宣言
+
+//オドメトリ用エンコーダープルアップ抵抗有効化
+DigitalIn Enc1_A(p26);
+DigitalIn Enc1_B(p25);
+DigitalIn Enc2_A(p24);
+DigitalIn Enc2_B(p23);
+DigitalIn Enc3_A(p22);
+DigitalIn Enc3_B(p21);
+QEI odo1(p26, p25,NC,resolution);
+QEI odo2(p24, p23,NC,resolution);
+QEI odo3(p22, p21,NC,resolution);
+
+RoboClaw roboclaw1(0x80,230400, p13, p14);
+RoboClaw roboclaw2(0x81,115200, p9, p10);
+
+//グローバル変数宣言
+double x_global=0.0,y_global=0.0,angle=0.0,t=0.0,vertical_degree=0.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 setup_omni(){
+    //オドメトリ用エンコーダープルアップ抵抗有効化
+    Enc1_A.mode(PullUp);
+    Enc1_B.mode(PullUp);
+    Enc2_A.mode(PullUp);
+    Enc2_B.mode(PullUp);
+    Enc3_A.mode(PullUp);
+    Enc3_B.mode(PullUp);
+    
+    //オドメトリ関数の時間割り込み有効化
+    tick_odo.attach(&odometry,dt);
+    
+    //モーター初期化
+    omni_move(0.0,0.0,0.0);
+}
+
+
+/*
+int main() {
+    setup();
+    wait(4.0);
+     while(1){
+        omni_move(100.0,100.0,0.0);
+        pc.printf("\nx_global:%lf\ty_global:%lf\tangle:%lf\t\n",x_global,y_global,angle);
+        wait(dt);
+     }
+}
+*/
+
+//－－－－－－－以下サブ関数－－－－－－－//
+double d_r(double degree){//度数→ラジアン
+    return degree*Pi/180.0;
+    }
+
+double r_d(double radian){//ラジアン→度数
+    return radian*180.0/Pi;
+    }
+
+double max_min(double val,double max,double min){//限度設定関数
+    if(val>max)return max;
+    else if(val<min)return min;
+    else return val;
+    }
+
+double mms_qpps(double millimeter_sec){//mm/sにqppsを変換
+    return millimeter_sec*((2024.0*4.0)/(radius*Pi));
+    }
+
+double odo_way(int mode){//pulseからmmに変換
+    if(mode==1) return ((double)odo1.getPulses()*omni1*Pi)/(resolution*en_bai);
+    else if(mode==2) return ((double)odo2.getPulses()*omni2*Pi)/(resolution*en_bai);
+    else if(mode==3) return ((double)odo3.getPulses()*omni3*Pi)/(resolution*en_bai);
+    else return 0;
+    }
+    
+void odometry(){ //単位時間あたりの移動量を積分して自己位置を推定する,神が書いた領域である関数
+    static double radian,pre_odo_way1=0.0,pre_odo_way2=0.0,pre_odo_way3=0.0;
+    static double xg_dot, yg_dot, prev_xg_dot = 0, prev_yg_dot = 0, prev_theta_dot = 0;
+    double x_dot,y_dot,v1,v2,v3,theta_dot;
+    double dw[4];
+    
+    dw[1] = odo_way(1);
+    dw[2] = odo_way(2);
+    dw[3] = odo_way(3);
+    
+    //三角関数式
+    radian=(dw[1]+dw[2]+dw[3])/(3*gaisetuen);
+    //way_y=(dw[1]*cos(radian) + dw[2]*cos(radian+d_r(120.0)) + dw[3]*cos(radian+d_r(240.0)))/2.0;
+    //way_x=(dw[1]*sin(radian) + dw[2]*sin(radian+d_r(120.0)) + dw[3]*sin(radian+d_r(240.0)))/2.0;   
+    
+    //積分式
+    v1 = (dw[1] - pre_odo_way1)/dt;
+    v2 = (dw[2] - pre_odo_way2)/dt;
+    v3 = (dw[3] - pre_odo_way3)/dt;
+    
+    x_dot=(v2-v3)/1.7320508;
+    y_dot=(-v1 + 2.0*(v2 + v3))/3.0;
+    theta_dot=(v1+v2+v3)/(3.0*gaisetuen/2.0);
+    
+    xg_dot=x_dot*cos(radian) - y_dot*sin(radian);
+    yg_dot=x_dot*sin(radian) + y_dot*cos(radian) - theta_dot/(gaisetuen/2.0);
+    
+    y_global += ((xg_dot + prev_xg_dot)/2.0)*dt;
+    x_global -= ((yg_dot + prev_yg_dot)/2.0)*dt;
+    radian += ((theta_dot + prev_theta_dot)/2.0)*dt;
+    
+    //y_global=(-1.0) * y_global;//値が常に0になるエラー発生
+    
+    prev_xg_dot = xg_dot;
+    prev_yg_dot = yg_dot;
+    prev_theta_dot = theta_dot;
+    pre_odo_way1=dw[1];
+    pre_odo_way2=dw[2];
+    pre_odo_way3=dw[3];
+    angle=(-1.0)*r_d(radian);
+}
+
+void omni_move(double x_speed,double y_speed,double angle){
+    x_speed=mms_qpps(x_speed);
+    y_speed=mms_qpps(y_speed);
+    angle=mms_qpps(angle);
+    roboclaw1.SpeedM2((int32_t)(x_speed*sin(d_r(90.0)) + y_speed*cos(d_r(90.0)) + angle));
+    roboclaw2.SpeedM1((int32_t)(x_speed*sin(d_r(330.0)) + y_speed*cos(d_r(330.0)) + angle));
+    roboclaw1.SpeedM1((int32_t)(-1.0*(x_speed*sin(d_r(210.0)) + y_speed*cos(d_r(210.0)) + angle)));
+    }
+    
+   
\ No newline at end of file