Wataru Shibazaki / Mbed 2 deprecated MiniRobocon

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?

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 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 }