Daiki Morita
/
2017-07-07
all publish
Revision 0:4442b3ee4c32, committed 2019-05-06
- Comitter:
- daikinect
- Date:
- Mon May 06 00:22:07 2019 +0000
- Commit message:
- all publish
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Mon May 06 00:22:07 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Motor/#f265e441bcd9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon May 06 00:22:07 2019 +0000 @@ -0,0 +1,78 @@ +#include "mbed.h" +#include "Motor.h" + +#define PI 3.14159265359 +#define Size_Of_Data 5 +#define Plus_F 'A' +#define Minus_F 'X' +double theta_robot=0.0; + +int i = 0; +int j; +int xbee_read_data[Size_Of_Data]; + +signed int Direction_Of_Rotation_X,Direction_Of_Rotation_Y; +double Speed_X,Speed_Y,Speed_wheel[3]; + +////////////////////////// +Serial Xbee(D1,D0); +Serial pc(USBTX,USBRX); +DigitalOut led(D13); +BusOut GNDs(D5,D9); +Motor wheel[3]= { + Motor(D2,D3,D4), //GND=GND + Motor(D6,D7,D8), //GND=D5 + Motor(D10,D11,D12) //GND=D9 +}; +////////////////////////// + +//データ変換 +void Char_To_Useful(int *array) +{ + Direction_Of_Rotation_X=(array[0]==Plus_F)-(array[0]==Minus_F); + Direction_Of_Rotation_Y=(array[1]==Plus_F)-(array[1]==Minus_F); + Speed_X=double(Direction_Of_Rotation_X)*array[2]/255.0; + Speed_Y=double(Direction_Of_Rotation_Y)*array[3]/255.0; + double wheel_v=sqrt(Speed_Y*Speed_Y+Speed_X*Speed_X); + double theta_v=atan2(Speed_Y,Speed_X); + for(int i=0; i<3; i++) + Speed_wheel[i]=wheel_v*sin(theta_v-theta_robot+PI*2.0/3.0*i); +} + + +//受信関数 +void rxFunc() +{ + //↓データ取得 + j = Xbee.getc(); + //↓データの個数が規定以下なら読み込み次に欲しいデータ番号を送る + if ( i < Size_Of_Data ) { + xbee_read_data[i] = j; + i++; + }//↓もしデータの個数が規定以上なら最初から送ってもらうために0を送る + if(i>=Size_Of_Data) { + i=0; + //led=1; + } + //↓欲しいデータの番号送信(iは次欲しいデータの番号) + Xbee.printf("%c",i); + led=!led; +} + +int main() +{ + GNDs=0; + Xbee.baud(9600); + Xbee.attach(&rxFunc,Serial::RxIrq); + Xbee.printf("%c",0); + while(1) { + if(xbee_read_data[Size_Of_Data-1]==1) { + Char_To_Useful(xbee_read_data); + for(int i=0; i<3; i++) { + wheel[i].speed(Speed_wheel[i]); + pc.printf("%f\n",/*Speed_X*/Speed_wheel[i]); + } + xbee_read_data[Size_Of_Data-1]=0; + } + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon May 06 00:22:07 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/22da6e220af6 \ No newline at end of file