Wataru Shibazaki / Mbed 2 deprecated MiniRobocon

LPC

Dependencies:   mbed MotorDrivers

main.cpp@1:b4ca9ec7b90b, 2020-10-10 (annotated)

Committer:
shibazakiwataru
Date:
Sat Oct 10 02:36:16 2020 +0000
Revision:
1:b4ca9ec7b90b
Parent:
0:13467eba9ca0
Child:
2:278a4347fd69
aaaa

Who changed what in which revision?

UserRevisionLine numberNew 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 1:b4ca9ec7b90b 7 #define MAX_QPPS1 40960
shibazakiwataru 1:b4ca9ec7b90b 8 #define MAX_QPPS2 40960
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 1:b4ca9ec7b90b 30 int motoXV,motoYV,motoRV;
shibazakiwataru 0:13467eba9ca0 31
shibazakiwataru 1:b4ca9ec7b90b 32 string reciv_str = "";//trasからtransへ変更予定
shibazakiwataru 0:13467eba9ca0 33
shibazakiwataru 1:b4ca9ec7b90b 34 void atrans(){//読み取り処理
shibazakiwataru 1:b4ca9ec7b90b 35 reciv_c = reciv.getc();//シリアル受信
shibazakiwataru 1:b4ca9ec7b90b 36 reciv_str += reciv_c;
shibazakiwataru 1:b4ca9ec7b90b 37 led1 = 1;
shibazakiwataru 1:b4ca9ec7b90b 38 led2 = 0;
shibazakiwataru 1:b4ca9ec7b90b 39 led3 = 0;
shibazakiwataru 1:b4ca9ec7b90b 40 led4 = 0;
shibazakiwataru 1:b4ca9ec7b90b 41 if(reciv_c == '\n'){//末尾改行コード（\n）感知
shibazakiwataru 1:b4ca9ec7b90b 42 motoX = reciv_str[0];
shibazakiwataru 1:b4ca9ec7b90b 43 motoY = reciv_str[1];
shibazakiwataru 1:b4ca9ec7b90b 44 motoR = reciv_str[2];
shibazakiwataru 1:b4ca9ec7b90b 45 button = reciv_str[3];
shibazakiwataru 1:b4ca9ec7b90b 46 reciv_str = "";
shibazakiwataru 1:b4ca9ec7b90b 47 led1 = 0;
shibazakiwataru 1:b4ca9ec7b90b 48 led2 = 1;
shibazakiwataru 1:b4ca9ec7b90b 49 led3 = 0;
shibazakiwataru 1:b4ca9ec7b90b 50 led4 = 0;
shibazakiwataru 1:b4ca9ec7b90b 51
shibazakiwataru 1:b4ca9ec7b90b 52
shibazakiwataru 0:13467eba9ca0 53 }
shibazakiwataru 0:13467eba9ca0 54 }
shibazakiwataru 0:13467eba9ca0 55 void aUSBtx(){//書き込み処理
shibazakiwataru 1:b4ca9ec7b90b 56 pc.printf("=%d,%d,%d",motoX,motoY,motoR);
shibazakiwataru 1:b4ca9ec7b90b 57 pc.printf(",%d",button);
shibazakiwataru 1:b4ca9ec7b90b 58 recivsome.putc(button);//上半身送信
shibazakiwataru 1:b4ca9ec7b90b 59 recivsome.putc('\n');
shibazakiwataru 1:b4ca9ec7b90b 60 pc.printf("\n");
shibazakiwataru 1:b4ca9ec7b90b 61 motoXV = motoX - 127;
shibazakiwataru 1:b4ca9ec7b90b 62 motoYV = motoY - 127;
shibazakiwataru 1:b4ca9ec7b90b 63 motoRV = motoR - 127;
shibazakiwataru 1:b4ca9ec7b90b 64
shibazakiwataru 1:b4ca9ec7b90b 65 int V1 = motoXV+motoRV;
shibazakiwataru 1:b4ca9ec7b90b 66 int V2 = motoXV/-2 + ruto_3*motoYV/2 + motoRV;//V2,V3の計算(三平方の定理)
shibazakiwataru 1:b4ca9ec7b90b 67 int V3 = motoXV/-2 - ruto_3*motoYV/2 + motoRV;
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 1:b4ca9ec7b90b 73 void Roboclaw_send(int V1,int V2,int V3){
shibazakiwataru 1:b4ca9ec7b90b 74 MD.SpeedM1(ADRS1,V1*int(MAX_QPPS1));
shibazakiwataru 1:b4ca9ec7b90b 75 MD.SpeedM2(ADRS2,V2*int(MAX_QPPS2));
shibazakiwataru 1:b4ca9ec7b90b 76 MD.SpeedM1(ADRS2,V3*int(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 }