PS3下の階
Dependencies: FatFileSystem PS3_BlueUSB QEI RoboClaw_mine_ mbed
Fork of PS3_BlueUSB by
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