kouhei umezawa / Mbed 2 deprecated 2022_NHK_B_UK

2022 NHK Bteam main totyuu

Dependencies:   HOSOKIkikou FEP_RX22 QEI R1370 ikarashiMDC_2byte_ver mbed 2022_NHK_B_canTR PID SEKIkikou

main.cpp@11:5697197abb4c, 20 months ago (annotated)

Committer:
umekou
Date:
Thu Oct 13 14:05:01 2022 +0000
Revision:
11:5697197abb4c
Parent:
10:9623a4b1f2b1 
final ver.

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"
ikarashikota 0:877aeb04390a 3 #include "pinconfig.h"
ikarashikota 0:877aeb04390a 4 #include "ikarashiMDC.h"
umekou 3:75dae8deb5f3 5 #include "QEI.h"
umekou 5:2934a132d594 6 #include "PID.h"
umekou 5:2934a132d594 7 #include "can_tr.h"
umekou 7:76790bcece4b 8 #include "SEKIkikou.h"
umekou 6:e7d542a2e49e 9 //#include "HOSOKIkikou.h"
ikarashikota 0:877aeb04390a 10
umekou 4:3c04f88a8cd8 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 7:76790bcece4b 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 5:2934a132d594 18 PID front(2.0, 0, 0.001, 0.01);
ikarashikota 0:877aeb04390a 19 DigitalOut stop(stop_pin);
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 7:76790bcece4b 30 ikarashiMDC(1,3,SM,&serial)
ikarashikota 0:877aeb04390a 31 };
ikarashikota 0:877aeb04390a 32
umekou 4:3c04f88a8cd8 33 uint8_t data[128];
umekou 5:2934a132d594 34 double frontAngle=0;
umekou 4:3c04f88a8cd8 35
ikarashikota 0:877aeb04390a 36 void recieveController(){
umekou 4:3c04f88a8cd8 37 mycon.getData(data);
umekou 5:2934a132d594 38 for (int i=7,j=128; j>0; i--,j/=2){
umekou 5:2934a132d594 39 b[i]=data[0]/j;
umekou 5:2934a132d594 40 data[0]%=j;
umekou 5:2934a132d594 41 }
umekou 5:2934a132d594 42 // for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
umekou 6:e7d542a2e49e 43 for (int i=0; i<4; i++) stick[i]=128-data[i+1];
umekou 6:e7d542a2e49e 44 if(mycon.getStatus()==false){
umekou 6:e7d542a2e49e 45 for (int i=0; i<8; i++) b[i]=0;
umekou 6:e7d542a2e49e 46 for (int i=0; i<4; i++) stick[i]=0;
umekou 6:e7d542a2e49e 47 }
umekou 8:e1f1a91e9353 48 /*
umekou 8:e1f1a91e9353 49 for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
umekou 8:e1f1a91e9353 50 pc.printf(" | ");
umekou 8:e1f1a91e9353 51 if (mycon.getStatus()) pc.printf("received\r\n");
umekou 8:e1f1a91e9353 52 else pc.printf("anything error...\r\n");*/
umekou 5:2934a132d594 53 }
umekou 5:2934a132d594 54
umekou 5:2934a132d594 55 void updateenc(){
umekou 7:76790bcece4b 56 // encoderValue[0] = (float)enc1.getPulses();
umekou 7:76790bcece4b 57 encoderValue[4] = (float)enc2.getPulses();
umekou 7:76790bcece4b 58 encoderValue[5] = (float)enc3.getPulses();
umekou 7:76790bcece4b 59 encoderValue[6] = (float)enc4.getPulses();
umekou 5:2934a132d594 60 // for (int i=0; i<4; i++) pc.printf("%d ", encoderValue[i]);
umekou 5:2934a132d594 61 // pc.printf("\r\n ");
ikarashikota 0:877aeb04390a 62 }
ikarashikota 0:877aeb04390a 63
umekou 5:2934a132d594 64 void allanglemove(double one,double two,double three,double four){
umekou 5:2934a132d594 65 if(stick[0]>20||stick[0]<-20||stick[1]>20||stick[1]<-20){//全方位移動
umekou 7:76790bcece4b 66 for(int i=0; i < 4; i++){
umekou 8:e1f1a91e9353 67 motorSpeed[i+8] = sin((atan2((double)stick[0],(double)stick[1])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
umekou 5:2934a132d594 68 }
umekou 5:2934a132d594 69 } else {
umekou 7:76790bcece4b 70 motorSpeed[8]=one;
umekou 7:76790bcece4b 71 motorSpeed[9]=two;
umekou 7:76790bcece4b 72 motorSpeed[10]=three;
umekou 7:76790bcece4b 73 motorSpeed[11]=four;
umekou 5:2934a132d594 74 }
umekou 4:3c04f88a8cd8 75 }
ikarashikota 0:877aeb04390a 76
umekou 5:2934a132d594 77 void PIDset(){
umekou 5:2934a132d594 78 front.setInputLimits (-180,180);
umekou 5:2934a132d594 79 front.setOutputLimits (-1,1);
umekou 5:2934a132d594 80 front.setBias(0);
umekou 5:2934a132d594 81 front.setMode (1);
umekou 5:2934a132d594 82 front.setSetPoint(0);
umekou 5:2934a132d594 83 }
umekou 5:2934a132d594 84
umekou 5:2934a132d594 85 void addPID(){
umekou 5:2934a132d594 86 front.setProcessValue (frontAngle);
umekou 7:76790bcece4b 87 for(int i=8; i < 12; i++){
umekou 5:2934a132d594 88 motorSpeed[i]-=front.compute();
umekou 5:2934a132d594 89 }
umekou 6:e7d542a2e49e 90 }
umekou 6:e7d542a2e49e 91
ikarashikota 0:877aeb04390a 92 int main()
ikarashikota 0:877aeb04390a 93 {
ikarashikota 0:877aeb04390a 94 mycon.StartReceive();
umekou 8:e1f1a91e9353 95 recieveController();
ikarashikota 0:877aeb04390a 96 stop = 1;
ikarashikota 0:877aeb04390a 97 led = 1;
ikarashikota 0:877aeb04390a 98
umekou 4:3c04f88a8cd8 99 printf("success!\r\n");
umekou 4:3c04f88a8cd8 100
umekou 8:e1f1a91e9353 101 for(int i=0; i < 8; i++){
umekou 8:e1f1a91e9353 102 motor[i].setSpeed(0);
umekou 8:e1f1a91e9353 103 }
umekou 8:e1f1a91e9353 104
umekou 8:e1f1a91e9353 105 // canAllReset();
umekou 5:2934a132d594 106 PIDset();
umekou 7:76790bcece4b 107 SEKIkikou seki(&motor[6], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
umekou 5:2934a132d594 108
ikarashikota 0:877aeb04390a 109 while(1) {
umekou 5:2934a132d594 110 recieveController();
umekou 8:e1f1a91e9353 111 canTR();
umekou 5:2934a132d594 112 updateenc();
umekou 8:e1f1a91e9353 113 frontAngle=jyroValue;
umekou 6:e7d542a2e49e 114
umekou 9:88f6351221ed 115 allanglemove(0,0,0,0);
umekou 9:88f6351221ed 116 addPID();
umekou 8:e1f1a91e9353 117
umekou 8:e1f1a91e9353 118 if(b[5]&&b[6]){
umekou 8:e1f1a91e9353 119 motorSpeed[15]=0;
umekou 8:e1f1a91e9353 120 }else if(b[5]){
umekou 8:e1f1a91e9353 121 motorSpeed[15]=0.5;
umekou 8:e1f1a91e9353 122 }else if(b[6]){
umekou 8:e1f1a91e9353 123 motorSpeed[15]=-0.5;
umekou 8:e1f1a91e9353 124 }else{
umekou 8:e1f1a91e9353 125 motorSpeed[15]=0;
umekou 8:e1f1a91e9353 126 }
umekou 8:e1f1a91e9353 127
umekou 10:9623a4b1f2b1 128 //関機構
umekou 8:e1f1a91e9353 129
umekou 8:e1f1a91e9353 130 if(b[7]==1){
umekou 10:9623a4b1f2b1 131 seki.runAll(0.1,0.3,-0.2);
umekou 8:e1f1a91e9353 132 }else{
umekou 11:5697197abb4c 133 seki.runAll(-0.8,0.3,0.3);
umekou 6:e7d542a2e49e 134 }
umekou 6:e7d542a2e49e 135
umekou 10:9623a4b1f2b1 136 //モータースピード設定
umekou 10:9623a4b1f2b1 137
umekou 7:76790bcece4b 138 for(int i=0; i < 4; i++){
umekou 7:76790bcece4b 139 motor[i].setSpeed(motorSpeed[i+8]*0.8);
umekou 6:e7d542a2e49e 140 }
umekou 6:e7d542a2e49e 141
umekou 8:e1f1a91e9353 142 motor[7].setSpeed(motorSpeed[15]);
umekou 8:e1f1a91e9353 143
umekou 10:9623a4b1f2b1 144 //以下デバッグ用pc.printf
umekou 10:9623a4b1f2b1 145
umekou 8:e1f1a91e9353 146 pc.printf("|wheel: ");
umekou 8:e1f1a91e9353 147 for(int i=8; i < 12; i++){
umekou 8:e1f1a91e9353 148 pc.printf("%.2f ",motorSpeed[i]);
umekou 5:2934a132d594 149 }
umekou 8:e1f1a91e9353 150 pc.printf("|spin: %.2f ",motorSpeed[15]);
umekou 8:e1f1a91e9353 151 // pc.printf("|updown: %.2f ",motorSpeed[2]);
umekou 8:e1f1a91e9353 152 pc.printf("|enc: ");
umekou 8:e1f1a91e9353 153 for(int i=0; i < 7; i++){
umekou 8:e1f1a91e9353 154 pc.printf("%d ",encoderValue[i]);
umekou 8:e1f1a91e9353 155
umekou 5:2934a132d594 156 }
umekou 8:e1f1a91e9353 157 pc.printf("| ");
umekou 8:e1f1a91e9353 158 pc.printf("R1370:%.3f",jyroValue);
umekou 8:e1f1a91e9353 159 pc.printf("| ");
umekou 8:e1f1a91e9353 160 for(int i=0; i < 8; i++){
umekou 8:e1f1a91e9353 161 pc.printf("%d ",b[i]);
umekou 5:2934a132d594 162 }
umekou 8:e1f1a91e9353 163 pc.printf("| ");
umekou 8:e1f1a91e9353 164 for(int i=0; i < 4; i++){
umekou 8:e1f1a91e9353 165 pc.printf("%d ",stick[i]);
umekou 5:2934a132d594 166 }
umekou 8:e1f1a91e9353 167 pc.printf("\r\n");
ikarashikota 0:877aeb04390a 168
ikarashikota 0:877aeb04390a 169 }
ikarashikota 0:877aeb04390a 170 }