LPC
Dependencies: mbed MotorDrivers
main.cpp@2:278a4347fd69, 2020-10-11 (annotated)
- Committer:
- shibazakiwataru
- Date:
- Sun Oct 11 08:47:33 2020 +0000
- Revision:
- 2:278a4347fd69
- Parent:
- 1:b4ca9ec7b90b
shibazaki_proguram_ashi
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shibazakiwataru | 0:13467eba9ca0 | 1 | #include "mbed.h" |
shibazakiwataru | 0:13467eba9ca0 | 2 | #include "string" |
shibazakiwataru | 1:b4ca9ec7b90b | 3 | #include "RoboClaw.h" |
shibazakiwataru | 0:13467eba9ca0 | 4 | //1272552550b00000001\n受信例 |
shibazakiwataru | 1:b4ca9ec7b90b | 5 | |
shibazakiwataru | 1:b4ca9ec7b90b | 6 | #define ruto_3 1.73205080757//√3 |
shibazakiwataru | 2:278a4347fd69 | 7 | #define MAX_QPPS1 40960/3 |
shibazakiwataru | 2:278a4347fd69 | 8 | #define MAX_QPPS2 40960/3 |
shibazakiwataru | 1:b4ca9ec7b90b | 9 | #define ADRS1 130 |
shibazakiwataru | 1:b4ca9ec7b90b | 10 | #define ADRS2 129 |
shibazakiwataru | 1:b4ca9ec7b90b | 11 | #define TIME 0.02 |
shibazakiwataru | 1:b4ca9ec7b90b | 12 | |
shibazakiwataru | 1:b4ca9ec7b90b | 13 | DigitalOut led1(LED1); |
shibazakiwataru | 1:b4ca9ec7b90b | 14 | DigitalOut led2(LED2); |
shibazakiwataru | 1:b4ca9ec7b90b | 15 | DigitalOut led3(LED3); |
shibazakiwataru | 1:b4ca9ec7b90b | 16 | DigitalOut led4(LED4); |
shibazakiwataru | 1:b4ca9ec7b90b | 17 | |
shibazakiwataru | 1:b4ca9ec7b90b | 18 | RawSerial reciv(p9,p10,115200); |
shibazakiwataru | 1:b4ca9ec7b90b | 19 | RawSerial recivsome(p28,p27,115200); |
shibazakiwataru | 1:b4ca9ec7b90b | 20 | RoboClaw MD(115200,p13,p14); |
shibazakiwataru | 0:13467eba9ca0 | 21 | RawSerial pc (USBTX,USBRX,115200); |
shibazakiwataru | 1:b4ca9ec7b90b | 22 | |
shibazakiwataru | 1:b4ca9ec7b90b | 23 | Ticker trans;//trasからtransへ変更予定trans=transmission(送信) |
shibazakiwataru | 0:13467eba9ca0 | 24 | |
shibazakiwataru | 0:13467eba9ca0 | 25 | char motoX;//モーターX軸 |
shibazakiwataru | 0:13467eba9ca0 | 26 | char motoY;//モーターY軸 |
shibazakiwataru | 0:13467eba9ca0 | 27 | char motoR;//モーター回転数 |
shibazakiwataru | 0:13467eba9ca0 | 28 | char button;//ボタン信号 上半身へ送信 |
shibazakiwataru | 1:b4ca9ec7b90b | 29 | char reciv_c; |
shibazakiwataru | 0:13467eba9ca0 | 30 | |
shibazakiwataru | 1:b4ca9ec7b90b | 31 | string reciv_str = "";//trasからtransへ変更予定 |
shibazakiwataru | 0:13467eba9ca0 | 32 | |
shibazakiwataru | 1:b4ca9ec7b90b | 33 | void atrans(){//読み取り処理 |
shibazakiwataru | 1:b4ca9ec7b90b | 34 | reciv_c = reciv.getc();//シリアル受信 |
shibazakiwataru | 1:b4ca9ec7b90b | 35 | reciv_str += reciv_c; |
shibazakiwataru | 1:b4ca9ec7b90b | 36 | led1 = 1; |
shibazakiwataru | 1:b4ca9ec7b90b | 37 | led2 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 38 | led3 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 39 | led4 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 40 | if(reciv_c == '\n'){//末尾改行コード(\n)感知 |
shibazakiwataru | 2:278a4347fd69 | 41 | motoY = reciv_str[0]; |
shibazakiwataru | 2:278a4347fd69 | 42 | motoX = reciv_str[1]; |
shibazakiwataru | 1:b4ca9ec7b90b | 43 | motoR = reciv_str[2]; |
shibazakiwataru | 1:b4ca9ec7b90b | 44 | button = reciv_str[3]; |
shibazakiwataru | 1:b4ca9ec7b90b | 45 | reciv_str = ""; |
shibazakiwataru | 1:b4ca9ec7b90b | 46 | led1 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 47 | led2 = 1; |
shibazakiwataru | 1:b4ca9ec7b90b | 48 | led3 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 49 | led4 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 50 | |
shibazakiwataru | 1:b4ca9ec7b90b | 51 | |
shibazakiwataru | 0:13467eba9ca0 | 52 | } |
shibazakiwataru | 0:13467eba9ca0 | 53 | } |
shibazakiwataru | 0:13467eba9ca0 | 54 | void aUSBtx(){//書き込み処理 |
shibazakiwataru | 1:b4ca9ec7b90b | 55 | pc.printf("=%d,%d,%d",motoX,motoY,motoR); |
shibazakiwataru | 2:278a4347fd69 | 56 | //pc.printf(",%d",button); |
shibazakiwataru | 1:b4ca9ec7b90b | 57 | recivsome.putc(button);//上半身送信 |
shibazakiwataru | 1:b4ca9ec7b90b | 58 | recivsome.putc('\n'); |
shibazakiwataru | 1:b4ca9ec7b90b | 59 | pc.printf("\n"); |
shibazakiwataru | 2:278a4347fd69 | 60 | int motoXV = motoX - 127; |
shibazakiwataru | 2:278a4347fd69 | 61 | int motoYV = motoY - 127; |
shibazakiwataru | 2:278a4347fd69 | 62 | int motoRV = motoR - 127; |
shibazakiwataru | 1:b4ca9ec7b90b | 63 | |
shibazakiwataru | 2:278a4347fd69 | 64 | double V1 = (double)motoXV + (double)motoRV; |
shibazakiwataru | 2:278a4347fd69 | 65 | double V2 = -0.5*(double)motoXV + ruto_3*0.5*(double)motoYV + (double)motoRV; |
shibazakiwataru | 2:278a4347fd69 | 66 | double V3 = -0.5*(double)motoXV - ruto_3*0.5*(double)motoYV + (double)motoRV; |
shibazakiwataru | 2:278a4347fd69 | 67 | pc.printf("v1=%d,v2=%d,v3=%d",int(V1),int(V2),int(V3)); |
shibazakiwataru | 1:b4ca9ec7b90b | 68 | led1 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 69 | led2 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 70 | led3 = 1; |
shibazakiwataru | 1:b4ca9ec7b90b | 71 | led4 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 72 | } |
shibazakiwataru | 2:278a4347fd69 | 73 | void Roboclaw_send(double V1, double V2, double V3){ |
shibazakiwataru | 2:278a4347fd69 | 74 | MD.SpeedM1(ADRS1,int(V3*MAX_QPPS1)); |
shibazakiwataru | 2:278a4347fd69 | 75 | MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1)); |
shibazakiwataru | 2:278a4347fd69 | 76 | MD.SpeedM1(ADRS2,int(V1*MAX_QPPS2)); |
shibazakiwataru | 1:b4ca9ec7b90b | 77 | led1 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 78 | led2 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 79 | led3 = 0; |
shibazakiwataru | 1:b4ca9ec7b90b | 80 | led4 = 1; |
shibazakiwataru | 0:13467eba9ca0 | 81 | } |
shibazakiwataru | 0:13467eba9ca0 | 82 | int main() |
shibazakiwataru | 0:13467eba9ca0 | 83 | { |
shibazakiwataru | 1:b4ca9ec7b90b | 84 | trans.attach(&aUSBtx,TIME);//タイマー割込み |
shibazakiwataru | 0:13467eba9ca0 | 85 | |
shibazakiwataru | 1:b4ca9ec7b90b | 86 | reciv.attach(&atrans,RawSerial::RxIrq);//受信割込み |
shibazakiwataru | 0:13467eba9ca0 | 87 | while(1) { |
shibazakiwataru | 0:13467eba9ca0 | 88 | } |
shibazakiwataru | 0:13467eba9ca0 | 89 | } |