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@7:4e43530672df, 19 months ago (annotated)

Committer:
umekou
Date:
Thu Oct 13 03:23:21 2022 +0000
Revision:
7:4e43530672df
Parent:
6:b426dcaa7c78
Child:
8:cccc03be9714
10gatu 12nitini koutani setumei sita yatu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikarashikota 0:877aeb04390a 1 #include "mbed.h"
umekou 6:b426dcaa7c78 2 #include "FEP_RX22.h"
ikarashikota 0:877aeb04390a 3 #include "pinconfig.h"
ikarashikota 0:877aeb04390a 4 #include "ikarashiMDC.h"
umekou 3:75dae8deb5f3 5 #include "QEI.h"
umekou 5:38778dac6ea0 6 #include "can_tr.h"
umekou 6:b426dcaa7c78 7 #include "HOSOKIkikou.h"
umekou 7:4e43530672df 8 //#include "SEKIkikou.h"
umekou 6:b426dcaa7c78 9 #include "R1370.h"
ikarashikota 0:877aeb04390a 10
umekou 6:b426dcaa7c78 11 FEP_RX22 mycon(fepTX, fepRX, fepad);
umekou 2:ea17f9e04972 12 Serial pc(USBTX, USBRX, 115200);
ikarashikota 0:877aeb04390a 13 Serial serial(motorTX, motorRX, 115200);
umekou 5:38778dac6ea0 14 QEI enc1(encoder1_A, encoder1_B, NC, 100, QEI::X4_ENCODING);
umekou 3:75dae8deb5f3 15 QEI enc2(encoder2_A, encoder2_B, NC, 100, QEI::X4_ENCODING);
umekou 3:75dae8deb5f3 16 QEI enc3(encoder3_A, encoder3_B, NC, 100, QEI::X4_ENCODING);
umekou 3:75dae8deb5f3 17 QEI enc4(encoder4_A, encoder4_B, NC, 100, QEI::X4_ENCODING);
umekou 6:b426dcaa7c78 18 R1370 r1370(jyro_TX,jyro_RX);
umekou 6:b426dcaa7c78 19 DigitalOut Reset(jyro_RST);
ikarashikota 0:877aeb04390a 20 DigitalOut led(LED2);
ikarashikota 0:877aeb04390a 21
ikarashikota 0:877aeb04390a 22 ikarashiMDC motor[] = {
ikarashikota 0:877aeb04390a 23 ikarashiMDC(0,0,SM,&serial),
ikarashikota 0:877aeb04390a 24 ikarashiMDC(0,1,SM,&serial),
ikarashikota 0:877aeb04390a 25 ikarashiMDC(0,2,SM,&serial),
umekou 3:75dae8deb5f3 26 ikarashiMDC(0,3,SM,&serial),
umekou 3:75dae8deb5f3 27 ikarashiMDC(1,0,SM,&serial),
umekou 3:75dae8deb5f3 28 ikarashiMDC(1,1,SM,&serial),
umekou 3:75dae8deb5f3 29 ikarashiMDC(1,2,SM,&serial),
umekou 3:75dae8deb5f3 30 ikarashiMDC(1,3,SM,&serial),
ikarashikota 0:877aeb04390a 31 };
umekou 5:38778dac6ea0 32 /*
umekou 4:3c04f88a8cd8 33 uint8_t data[128];
umekou 4:3c04f88a8cd8 34
ikarashikota 0:877aeb04390a 35 void recieveController(){
umekou 4:3c04f88a8cd8 36 mycon.getData(data);
umekou 5:38778dac6ea0 37 for (int i=7,j=128; j>0; i--,j/=2){
umekou 5:38778dac6ea0 38 b[i]=data[0]/j;
umekou 5:38778dac6ea0 39 data[0]%=j;
umekou 5:38778dac6ea0 40 }
umekou 6:b426dcaa7c78 41 for (int i=0; i<4; i++) stick[i]=128-data[i+1];
umekou 6:b426dcaa7c78 42 if(mycon.getStatus()==false){
umekou 6:b426dcaa7c78 43 for (int i=0; i<8; i++) b[i]=0;
umekou 6:b426dcaa7c78 44 for (int i=0; i<4; i++) stick[i]=0;
umekou 6:b426dcaa7c78 45 }
umekou 6:b426dcaa7c78 46 canTR();
umekou 5:38778dac6ea0 47 for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
umekou 5:38778dac6ea0 48 for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
ikarashikota 0:877aeb04390a 49 pc.printf(" | ");
umekou 4:3c04f88a8cd8 50 if (mycon.getStatus()) pc.printf("received\r\n");
ikarashikota 0:877aeb04390a 51 else pc.printf("anything error...\r\n");
ikarashikota 0:877aeb04390a 52 }
umekou 5:38778dac6ea0 53 */
umekou 5:38778dac6ea0 54 void updateenc(){
umekou 6:b426dcaa7c78 55 encoderValue[0] = (float)enc1.getPulses();
umekou 6:b426dcaa7c78 56 encoderValue[1] = (float)enc2.getPulses();
umekou 6:b426dcaa7c78 57 encoderValue[2] = (float)enc3.getPulses();
umekou 6:b426dcaa7c78 58 encoderValue[3] = (float)enc4.getPulses();
umekou 6:b426dcaa7c78 59 // for (int i=4; i<8; i++) pc.printf("%d ", encoderValue[i]);
umekou 6:b426dcaa7c78 60 // pc.printf("\r\n ");
umekou 6:b426dcaa7c78 61 }
umekou 6:b426dcaa7c78 62
umekou 6:b426dcaa7c78 63 void updatejyro(){
umekou 6:b426dcaa7c78 64 r1370.update();
umekou 6:b426dcaa7c78 65 jyroValue=r1370.getAngle();
umekou 6:b426dcaa7c78 66 // pc.printf("%f ",jyroValue);
umekou 4:3c04f88a8cd8 67 }
ikarashikota 0:877aeb04390a 68
ikarashikota 0:877aeb04390a 69 int main()
ikarashikota 0:877aeb04390a 70 {
umekou 5:38778dac6ea0 71 // mycon.StartReceive();
ikarashikota 0:877aeb04390a 72 led = 1;
umekou 6:b426dcaa7c78 73 pc.printf("success!\r\n");
umekou 7:4e43530672df 74
umekou 7:4e43530672df 75 for(int i=0; i < 8; i++){
umekou 7:4e43530672df 76 motor[i].setSpeed(0);
umekou 7:4e43530672df 77 }
umekou 7:4e43530672df 78 // int bc=0,bn=0;
umekou 7:4e43530672df 79 // int currentMode=1;
umekou 7:4e43530672df 80 HOSOKIkikou hosoki(&motor[6],&motor[7], &motor[4], &motor[5], &b[4], NULL, NULL, NULL, NULL, NULL);
umekou 5:38778dac6ea0 81 while(1) {
umekou 5:38778dac6ea0 82 // recieveController();
umekou 6:b426dcaa7c78 83 Reset=0;
umekou 5:38778dac6ea0 84 updateenc();
umekou 6:b426dcaa7c78 85 updatejyro();
umekou 7:4e43530672df 86 /*
umekou 6:b426dcaa7c78 87 bc=b[7]-bn;
umekou 6:b426dcaa7c78 88 bn=b[7];
umekou 6:b426dcaa7c78 89
umekou 6:b426dcaa7c78 90 if(currentMode==1){
umekou 6:b426dcaa7c78 91 if(bc==1){
umekou 6:b426dcaa7c78 92 currentMode=2;
umekou 6:b426dcaa7c78 93 led = 1;
umekou 6:b426dcaa7c78 94 hosoki.stopAll();
umekou 6:b426dcaa7c78 95 }
umekou 6:b426dcaa7c78 96 }else if(currentMode==2){
umekou 6:b426dcaa7c78 97 if(bc==1){
umekou 6:b426dcaa7c78 98 currentMode=3;
umekou 6:b426dcaa7c78 99 led = 0;
umekou 6:b426dcaa7c78 100 hosoki.init(&motor[6],&motor[7], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[2], &encoderValue[3]);
umekou 6:b426dcaa7c78 101 }
umekou 6:b426dcaa7c78 102 }else if(currentMode==3){
umekou 6:b426dcaa7c78 103 if(bc==1){
umekou 6:b426dcaa7c78 104 currentMode=1;
umekou 6:b426dcaa7c78 105 led = 1;
umekou 6:b426dcaa7c78 106 hosoki.stopAll();
umekou 6:b426dcaa7c78 107 }else{
umekou 6:b426dcaa7c78 108 hosoki.runAll(-0.9,0.9,0.3,0.3);
umekou 6:b426dcaa7c78 109 }
umekou 6:b426dcaa7c78 110 }
umekou 7:4e43530672df 111 */
umekou 5:38778dac6ea0 112 canTR();
umekou 7:4e43530672df 113 /*
umekou 7:4e43530672df 114 if(stick[2]>80||stick[2]<-80||stick[3]>80||stick[3]<-80){//機構回転
umekou 7:4e43530672df 115 if(stick[2]>80){
umekou 7:4e43530672df 116 motorSpeed[1] = 0.5;
umekou 7:4e43530672df 117 }else if(stick[2]<-80){
umekou 7:4e43530672df 118 motorSpeed[1] = -0.5;
umekou 6:b426dcaa7c78 119 }
umekou 6:b426dcaa7c78 120 if(stick[3]>100){
umekou 6:b426dcaa7c78 121 motorSpeed[2] = 0.8;
umekou 6:b426dcaa7c78 122 }else if(stick[3]<-100){
umekou 6:b426dcaa7c78 123 motorSpeed[2] = -0.8;
umekou 6:b426dcaa7c78 124 }
umekou 6:b426dcaa7c78 125 } else {
umekou 6:b426dcaa7c78 126 motorSpeed[2]=0;
umekou 6:b426dcaa7c78 127 motorSpeed[1]=0;
umekou 7:4e43530672df 128 }*/
umekou 7:4e43530672df 129 hosoki.runAll(-0.9,0.9,0,0);
ikarashikota 0:877aeb04390a 130
umekou 7:4e43530672df 131 // for(int i=1; i < 3; i++){
umekou 7:4e43530672df 132 // motor[i].setSpeed(motorSpeed[i]);
umekou 7:4e43530672df 133 // }
umekou 6:b426dcaa7c78 134
umekou 7:4e43530672df 135 pc.printf("|wheel: ");
umekou 7:4e43530672df 136 for(int i=8; i < 12; i++){
umekou 7:4e43530672df 137 pc.printf("%.2f ",motorSpeed[i]);
umekou 6:b426dcaa7c78 138 }
umekou 7:4e43530672df 139 pc.printf("|spin: %.2f ",motorSpeed[1]);
umekou 7:4e43530672df 140 pc.printf("|updown: %.2f ",motorSpeed[2]);
umekou 7:4e43530672df 141 pc.printf("|enc: ");
umekou 7:4e43530672df 142 for(int i=0; i < 7; i++){
umekou 7:4e43530672df 143 pc.printf("%d ",encoderValue[i]);
umekou 7:4e43530672df 144
umekou 7:4e43530672df 145 }
umekou 7:4e43530672df 146 pc.printf("| ");
umekou 7:4e43530672df 147 pc.printf("R1370:%.3f",jyroValue);
umekou 7:4e43530672df 148 pc.printf("| ");
umekou 6:b426dcaa7c78 149 for(int i=0; i < 8; i++){
umekou 7:4e43530672df 150 pc.printf("%d ",b[i]);
umekou 6:b426dcaa7c78 151 }
umekou 6:b426dcaa7c78 152 pc.printf("| ");
umekou 6:b426dcaa7c78 153 for(int i=0; i < 4; i++){
umekou 6:b426dcaa7c78 154 pc.printf("%d ",stick[i]);
umekou 6:b426dcaa7c78 155 }
umekou 7:4e43530672df 156 pc.printf("\r\n");
umekou 6:b426dcaa7c78 157
ikarashikota 0:877aeb04390a 158 }
ikarashikota 0:877aeb04390a 159 }