NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2022_NHK_B_sub2

sub2 MDC and enc

Dependencies:   2022_NHK_B_canTR FEP_RX22 QEI ikarashiMDC_2byte_ver mbed HOSOKIkikou R1370 SEKIkikou

main.cpp@4:3c04f88a8cd8, 20 months ago (annotated)

Committer:
umekou
Date:
Sat Oct 08 08:31:07 2022 +0000
Revision:
4:3c04f88a8cd8
Parent:
3:75dae8deb5f3
Child:
5:38778dac6ea0
change enc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikarashikota 0:877aeb04390a 1 #include "mbed.h"
umekou 4:3c04f88a8cd8 2 #include "FEP_RX22.h"
umekou 4:3c04f88a8cd8 3 //#include "controller.h"
ikarashikota 0:877aeb04390a 4 #include "pinconfig.h"
ikarashikota 0:877aeb04390a 5 #include "ikarashiMDC.h"
umekou 3:75dae8deb5f3 6 #include "QEI.h"
ikarashikota 0:877aeb04390a 7
umekou 4:3c04f88a8cd8 8 FEP_RX22 mycon(fepTX, fepRX, fepad);
umekou 4:3c04f88a8cd8 9 //Bcon mycon(fepTX, fepRX, fepad);
umekou 2:ea17f9e04972 10 Serial pc(USBTX, USBRX, 115200);
ikarashikota 0:877aeb04390a 11 Serial serial(motorTX, motorRX, 115200);
umekou 4:3c04f88a8cd8 12 //QEI enc1(encoder1_A, encoder1_B, NC, 100, QEI::X4_ENCODING);
umekou 3:75dae8deb5f3 13 QEI enc2(encoder2_A, encoder2_B, NC, 100, QEI::X4_ENCODING);
umekou 3:75dae8deb5f3 14 QEI enc3(encoder3_A, encoder3_B, NC, 100, QEI::X4_ENCODING);
umekou 3:75dae8deb5f3 15 QEI enc4(encoder4_A, encoder4_B, NC, 100, QEI::X4_ENCODING);
ikarashikota 0:877aeb04390a 16 DigitalOut stop(stop_pin);
ikarashikota 0:877aeb04390a 17 DigitalOut led(LED2);
ikarashikota 0:877aeb04390a 18
ikarashikota 0:877aeb04390a 19 ikarashiMDC motor[] = {
ikarashikota 0:877aeb04390a 20 ikarashiMDC(0,0,SM,&serial),
ikarashikota 0:877aeb04390a 21 ikarashiMDC(0,1,SM,&serial),
ikarashikota 0:877aeb04390a 22 ikarashiMDC(0,2,SM,&serial),
umekou 3:75dae8deb5f3 23 ikarashiMDC(0,3,SM,&serial),
umekou 3:75dae8deb5f3 24 ikarashiMDC(1,0,SM,&serial),
umekou 3:75dae8deb5f3 25 ikarashiMDC(1,1,SM,&serial),
umekou 3:75dae8deb5f3 26 ikarashiMDC(1,2,SM,&serial),
umekou 3:75dae8deb5f3 27 ikarashiMDC(1,3,SM,&serial),
umekou 3:75dae8deb5f3 28 ikarashiMDC(2,0,SM,&serial),
umekou 3:75dae8deb5f3 29 ikarashiMDC(2,1,SM,&serial),
umekou 3:75dae8deb5f3 30 ikarashiMDC(2,2,SM,&serial),
umekou 4:3c04f88a8cd8 31 ikarashiMDC(2,3,SM,&serial),
umekou 4:3c04f88a8cd8 32 ikarashiMDC(3,0,SM,&serial)
ikarashikota 0:877aeb04390a 33 };
ikarashikota 0:877aeb04390a 34
ikarashikota 0:877aeb04390a 35 uint8_t b[8];
ikarashikota 0:877aeb04390a 36 int16_t stick[4];
ikarashikota 0:877aeb04390a 37
umekou 4:3c04f88a8cd8 38 uint8_t data[128];
umekou 4:3c04f88a8cd8 39
umekou 4:3c04f88a8cd8 40 float angle[4]={};
umekou 4:3c04f88a8cd8 41
umekou 4:3c04f88a8cd8 42 double speed[13]={};
umekou 4:3c04f88a8cd8 43
ikarashikota 0:877aeb04390a 44 void recieveController(){
umekou 4:3c04f88a8cd8 45 mycon.getData(data);
umekou 4:3c04f88a8cd8 46 for (int i=0; i<5; i++) pc.printf("%d ", data[i]);
ikarashikota 0:877aeb04390a 47 pc.printf(" | ");
umekou 4:3c04f88a8cd8 48 if (mycon.getStatus()) pc.printf("received\r\n");
ikarashikota 0:877aeb04390a 49 else pc.printf("anything error...\r\n");
ikarashikota 0:877aeb04390a 50 }
ikarashikota 0:877aeb04390a 51
umekou 4:3c04f88a8cd8 52 void updateangle(){
umekou 4:3c04f88a8cd8 53 //angle[0] = (float)enc1.getPulses();
umekou 4:3c04f88a8cd8 54 angle[1] = (float)enc2.getPulses();
umekou 4:3c04f88a8cd8 55 angle[2] = (float)enc3.getPulses();
umekou 4:3c04f88a8cd8 56 angle[3] = (float)enc4.getPulses();
umekou 4:3c04f88a8cd8 57 for (int i=0; i<4; i++) pc.printf("%f ", angle[i]);
umekou 4:3c04f88a8cd8 58 pc.printf("\r\n ");
umekou 4:3c04f88a8cd8 59 }
ikarashikota 0:877aeb04390a 60
ikarashikota 0:877aeb04390a 61 int main()
ikarashikota 0:877aeb04390a 62 {
ikarashikota 0:877aeb04390a 63 mycon.StartReceive();
ikarashikota 0:877aeb04390a 64 stop = 1;
ikarashikota 0:877aeb04390a 65 led = 1;
ikarashikota 0:877aeb04390a 66
umekou 4:3c04f88a8cd8 67 printf("success!\r\n");
umekou 4:3c04f88a8cd8 68
ikarashikota 0:877aeb04390a 69 while(1) {
ikarashikota 0:877aeb04390a 70
ikarashikota 0:877aeb04390a 71 //recieveController();
umekou 4:3c04f88a8cd8 72 updateangle();
umekou 4:3c04f88a8cd8 73
umekou 4:3c04f88a8cd8 74
ikarashikota 0:877aeb04390a 75
ikarashikota 0:877aeb04390a 76 }
ikarashikota 0:877aeb04390a 77 }