Daiki Morita
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