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@6:b426dcaa7c78, 20 months ago (annotated)

Committer:
umekou
Date:
Wed Oct 12 00:24:34 2022 +0000
Revision:
6:b426dcaa7c78
Parent:
5:38778dac6ea0
Child:
7:4e43530672df
jikken mada

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 6:b426dcaa7c78 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 6:b426dcaa7c78 74 int bc=0,bn=0;
umekou 6:b426dcaa7c78 75 int currentMode=1;
umekou 6:b426dcaa7c78 76 HOSOKIkikou hosoki(&motor[6],&motor[7], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[2], &encoderValue[3]);
umekou 5:38778dac6ea0 77 while(1) {
umekou 5:38778dac6ea0 78 // recieveController();
umekou 6:b426dcaa7c78 79 Reset=0;
umekou 5:38778dac6ea0 80 updateenc();
umekou 6:b426dcaa7c78 81 updatejyro();
umekou 6:b426dcaa7c78 82
umekou 6:b426dcaa7c78 83 bc=b[7]-bn;
umekou 6:b426dcaa7c78 84 bn=b[7];
umekou 6:b426dcaa7c78 85
umekou 6:b426dcaa7c78 86 if(currentMode==1){
umekou 6:b426dcaa7c78 87 if(bc==1){
umekou 6:b426dcaa7c78 88 currentMode=2;
umekou 6:b426dcaa7c78 89 led = 1;
umekou 6:b426dcaa7c78 90 hosoki.stopAll();
umekou 6:b426dcaa7c78 91 }
umekou 6:b426dcaa7c78 92 }else if(currentMode==2){
umekou 6:b426dcaa7c78 93 if(bc==1){
umekou 6:b426dcaa7c78 94 currentMode=3;
umekou 6:b426dcaa7c78 95 led = 0;
umekou 6:b426dcaa7c78 96 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 97 }
umekou 6:b426dcaa7c78 98 }else if(currentMode==3){
umekou 6:b426dcaa7c78 99 if(bc==1){
umekou 6:b426dcaa7c78 100 currentMode=1;
umekou 6:b426dcaa7c78 101 led = 1;
umekou 6:b426dcaa7c78 102 hosoki.stopAll();
umekou 6:b426dcaa7c78 103 }else{
umekou 6:b426dcaa7c78 104 hosoki.runAll(-0.9,0.9,0.3,0.3);
umekou 6:b426dcaa7c78 105 }
umekou 6:b426dcaa7c78 106 }
umekou 5:38778dac6ea0 107 canTR();
umekou 6:b426dcaa7c78 108
umekou 6:b426dcaa7c78 109 if(stick[2]>100||stick[2]<-100||stick[3]>100||stick[3]<-100){//機構回転
umekou 6:b426dcaa7c78 110 if(stick[2]>100){
umekou 6:b426dcaa7c78 111 motorSpeed[1] = 0.8;
umekou 6:b426dcaa7c78 112 }else if(stick[2]<-100){
umekou 6:b426dcaa7c78 113 motorSpeed[1] = -0.8;
umekou 6:b426dcaa7c78 114 }
umekou 6:b426dcaa7c78 115 if(stick[3]>100){
umekou 6:b426dcaa7c78 116 motorSpeed[2] = 0.8;
umekou 6:b426dcaa7c78 117 }else if(stick[3]<-100){
umekou 6:b426dcaa7c78 118 motorSpeed[2] = -0.8;
umekou 6:b426dcaa7c78 119 }
umekou 6:b426dcaa7c78 120 } else {
umekou 6:b426dcaa7c78 121 motorSpeed[2]=0;
umekou 6:b426dcaa7c78 122 motorSpeed[1]=0;
umekou 5:38778dac6ea0 123 }
ikarashikota 0:877aeb04390a 124
umekou 6:b426dcaa7c78 125
umekou 6:b426dcaa7c78 126 for(int i=1; i < 3; i++){
umekou 6:b426dcaa7c78 127 motor[i].setSpeed(motorSpeed[i]);
umekou 6:b426dcaa7c78 128 }
umekou 6:b426dcaa7c78 129
umekou 6:b426dcaa7c78 130 for(int i=0; i < 8; i++){
umekou 6:b426dcaa7c78 131 pc.printf("%d ",b[i]);
umekou 6:b426dcaa7c78 132 }
umekou 6:b426dcaa7c78 133 pc.printf("| ");
umekou 6:b426dcaa7c78 134 for(int i=0; i < 4; i++){
umekou 6:b426dcaa7c78 135 pc.printf("%d ",stick[i]);
umekou 6:b426dcaa7c78 136 }
umekou 6:b426dcaa7c78 137 pc.printf("| ");
umekou 6:b426dcaa7c78 138 for(int i=2; i < 15; i++){
umekou 6:b426dcaa7c78 139 pc.printf("%.3f ",motorSpeed[i]);
umekou 6:b426dcaa7c78 140 }
umekou 6:b426dcaa7c78 141 pc.printf("| ");
umekou 6:b426dcaa7c78 142 for(int i=0; i < 12; i++){
umekou 6:b426dcaa7c78 143 pc.printf("%d ",encoderValue[i]);
umekou 6:b426dcaa7c78 144 }
umekou 6:b426dcaa7c78 145 pc.printf("| ");
umekou 6:b426dcaa7c78 146 pc.printf("%.4f\r\n",jyroValue);
umekou 6:b426dcaa7c78 147
ikarashikota 0:877aeb04390a 148 }
ikarashikota 0:877aeb04390a 149 }