2018_Project-R
PS3下の階
Dependencies: FatFileSystem PS3_BlueUSB QEI RoboClaw_mine_ mbed
Fork of
PS3_BlueUSB
by 
Bart Janssens
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
--- 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;
--- 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
--- /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
--- 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);
+ }
--- /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
--- /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
--- 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");
--- 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;
--- 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();
}
--- /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