LPC
Dependencies: mbed MotorDrivers
Diff: main.cpp
- Revision:
- 2:278a4347fd69
- Parent:
- 1:b4ca9ec7b90b
--- a/main.cpp Sat Oct 10 02:36:16 2020 +0000 +++ b/main.cpp Sun Oct 11 08:47:33 2020 +0000 @@ -4,8 +4,8 @@ //1272552550b00000001\n受信例 #define ruto_3 1.73205080757//√3 -#define MAX_QPPS1 40960 -#define MAX_QPPS2 40960 +#define MAX_QPPS1 40960/3 +#define MAX_QPPS2 40960/3 #define ADRS1 130 #define ADRS2 129 #define TIME 0.02 @@ -27,7 +27,6 @@ char motoR;//モーター回転数 char button;//ボタン信号 上半身へ送信 char reciv_c; -int motoXV,motoYV,motoRV; string reciv_str = "";//trasからtransへ変更予定 @@ -39,8 +38,8 @@ led3 = 0; led4 = 0; if(reciv_c == '\n'){//末尾改行コード(\n)感知 - motoX = reciv_str[0]; - motoY = reciv_str[1]; + motoY = reciv_str[0]; + motoX = reciv_str[1]; motoR = reciv_str[2]; button = reciv_str[3]; reciv_str = ""; @@ -54,26 +53,27 @@ } void aUSBtx(){//書き込み処理 pc.printf("=%d,%d,%d",motoX,motoY,motoR); - pc.printf(",%d",button); + //pc.printf(",%d",button); recivsome.putc(button);//上半身送信 recivsome.putc('\n'); pc.printf("\n"); - motoXV = motoX - 127; - motoYV = motoY - 127; - motoRV = motoR - 127; + int motoXV = motoX - 127; + int motoYV = motoY - 127; + int motoRV = motoR - 127; - int V1 = motoXV+motoRV; - int V2 = motoXV/-2 + ruto_3*motoYV/2 + motoRV;//V2,V3の計算(三平方の定理) - int V3 = motoXV/-2 - ruto_3*motoYV/2 + motoRV; + double V1 = (double)motoXV + (double)motoRV; + double V2 = -0.5*(double)motoXV + ruto_3*0.5*(double)motoYV + (double)motoRV; + double V3 = -0.5*(double)motoXV - ruto_3*0.5*(double)motoYV + (double)motoRV; + pc.printf("v1=%d,v2=%d,v3=%d",int(V1),int(V2),int(V3)); led1 = 0; led2 = 0; led3 = 1; led4 = 0; } -void Roboclaw_send(int V1,int V2,int V3){ - MD.SpeedM1(ADRS1,V1*int(MAX_QPPS1)); - MD.SpeedM2(ADRS2,V2*int(MAX_QPPS2)); - MD.SpeedM1(ADRS2,V3*int(MAX_QPPS2)); +void Roboclaw_send(double V1, double V2, double V3){ + MD.SpeedM1(ADRS1,int(V3*MAX_QPPS1)); + MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1)); + MD.SpeedM1(ADRS2,int(V1*MAX_QPPS2)); led1 = 0; led2 = 0; led3 = 0;