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@8:e1f1a91e9353, 2022-10-12 (annotated)

Committer:
umekou
Date:
Wed Oct 12 16:05:05 2022 +0000
Revision:
8:e1f1a91e9353
Parent:
7:76790bcece4b
Child:
9:88f6351221ed
10gatu 12nitini ni koutani setumeisita dousawo suru puroguramudesu

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 // canTR();
umekou 8:e1f1a91e9353 49 /*
umekou 8:e1f1a91e9353 50 for (int i=0; i<4; i++) pc.printf("%d ", stick[i]);
umekou 8:e1f1a91e9353 51 pc.printf(" | ");
umekou 8:e1f1a91e9353 52 if (mycon.getStatus()) pc.printf("received\r\n");
umekou 8:e1f1a91e9353 53 else pc.printf("anything error...\r\n");*/
umekou 5:2934a132d594 54 }
umekou 5:2934a132d594 55
umekou 5:2934a132d594 56 void updateenc(){
umekou 7:76790bcece4b 57 // encoderValue[0] = (float)enc1.getPulses();
umekou 7:76790bcece4b 58 encoderValue[4] = (float)enc2.getPulses();
umekou 7:76790bcece4b 59 encoderValue[5] = (float)enc3.getPulses();
umekou 7:76790bcece4b 60 encoderValue[6] = (float)enc4.getPulses();
umekou 8:e1f1a91e9353 61 // canTR();
umekou 5:2934a132d594 62 // for (int i=0; i<4; i++) pc.printf("%d ", encoderValue[i]);
umekou 5:2934a132d594 63 // pc.printf("\r\n ");
ikarashikota 0:877aeb04390a 64 }
ikarashikota 0:877aeb04390a 65
umekou 5:2934a132d594 66 void allanglemove(double one,double two,double three,double four){
umekou 5:2934a132d594 67 if(stick[0]>20||stick[0]<-20||stick[1]>20||stick[1]<-20){//全方位移動
umekou 7:76790bcece4b 68 for(int i=0; i < 4; i++){
umekou 8:e1f1a91e9353 69 motorSpeed[i+8] = sin((atan2((double)stick[0],(double)stick[1])-3.14*(i*2-1) / 4)+(frontAngle*3.14/180));
umekou 5:2934a132d594 70 }
umekou 5:2934a132d594 71 } else {
umekou 7:76790bcece4b 72 motorSpeed[8]=one;
umekou 7:76790bcece4b 73 motorSpeed[9]=two;
umekou 7:76790bcece4b 74 motorSpeed[10]=three;
umekou 7:76790bcece4b 75 motorSpeed[11]=four;
umekou 5:2934a132d594 76 }
umekou 8:e1f1a91e9353 77 // canTR();
umekou 4:3c04f88a8cd8 78 }
ikarashikota 0:877aeb04390a 79
umekou 5:2934a132d594 80 void PIDset(){
umekou 5:2934a132d594 81 front.setInputLimits (-180,180);
umekou 5:2934a132d594 82 front.setOutputLimits (-1,1);
umekou 5:2934a132d594 83 front.setBias(0);
umekou 5:2934a132d594 84 front.setMode (1);
umekou 5:2934a132d594 85 front.setSetPoint(0);
umekou 5:2934a132d594 86 }
umekou 5:2934a132d594 87
umekou 5:2934a132d594 88 void addPID(){
umekou 5:2934a132d594 89 front.setProcessValue (frontAngle);
umekou 7:76790bcece4b 90 for(int i=8; i < 12; i++){
umekou 5:2934a132d594 91 motorSpeed[i]-=front.compute();
umekou 5:2934a132d594 92 }
umekou 8:e1f1a91e9353 93 // canTR();
umekou 6:e7d542a2e49e 94 }
umekou 6:e7d542a2e49e 95
ikarashikota 0:877aeb04390a 96 int main()
ikarashikota 0:877aeb04390a 97 {
ikarashikota 0:877aeb04390a 98 mycon.StartReceive();
umekou 8:e1f1a91e9353 99 recieveController();
ikarashikota 0:877aeb04390a 100 stop = 1;
ikarashikota 0:877aeb04390a 101 led = 1;
ikarashikota 0:877aeb04390a 102
umekou 8:e1f1a91e9353 103 // int currentMode=1;
umekou 8:e1f1a91e9353 104 // int bc=0,bn=0;
umekou 8:e1f1a91e9353 105 // int sa=0;
umekou 5:2934a132d594 106
umekou 4:3c04f88a8cd8 107 printf("success!\r\n");
umekou 4:3c04f88a8cd8 108
umekou 8:e1f1a91e9353 109 for(int i=0; i < 8; i++){
umekou 8:e1f1a91e9353 110 motor[i].setSpeed(0);
umekou 8:e1f1a91e9353 111 }
umekou 8:e1f1a91e9353 112
umekou 8:e1f1a91e9353 113 // canAllReset();
umekou 5:2934a132d594 114 PIDset();
umekou 7:76790bcece4b 115 SEKIkikou seki(&motor[6], &motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
umekou 8:e1f1a91e9353 116 // seki.stopAll();
umekou 8:e1f1a91e9353 117 // SEKIkikou seki(&motor[6], &motor[4], &motor[5], NULL, NULL, NULL, NULL, NULL, NULL, NULL);
umekou 8:e1f1a91e9353 118
umekou 8:e1f1a91e9353 119
umekou 8:e1f1a91e9353 120
umekou 5:2934a132d594 121
ikarashikota 0:877aeb04390a 122 while(1) {
umekou 5:2934a132d594 123 recieveController();
umekou 8:e1f1a91e9353 124 canTR();
umekou 5:2934a132d594 125 updateenc();
umekou 8:e1f1a91e9353 126 frontAngle=jyroValue;
umekou 6:e7d542a2e49e 127
umekou 8:e1f1a91e9353 128 /*
umekou 5:2934a132d594 129 bc=b[7]-bn;
umekou 5:2934a132d594 130 bn=b[7];
umekou 4:3c04f88a8cd8 131
umekou 5:2934a132d594 132 if(currentMode==1){
umekou 5:2934a132d594 133 if(bc==1){
umekou 5:2934a132d594 134 currentMode=2;
umekou 7:76790bcece4b 135 seki.init(&motor[6],&motor[4], &motor[5], &b[0], &b[1], &b[2], &b[3], &encoderValue[4], &encoderValue[5], &encoderValue[6]);
umekou 8:e1f1a91e9353 136 seki.runAll(0,0,0);
umekou 5:2934a132d594 137 allanglemove(0,0,0,0);
umekou 5:2934a132d594 138 }else if(b[0]){
umekou 5:2934a132d594 139 allanglemove(-1,-1,1,1);
umekou 5:2934a132d594 140 }else if(b[1]){
umekou 6:e7d542a2e49e 141 allanglemove(1,-1,-1,1);
umekou 5:2934a132d594 142 }else if(b[2]){
umekou 6:e7d542a2e49e 143 allanglemove(1,1,-1,-1);
umekou 5:2934a132d594 144 }else if(b[3]){
umekou 6:e7d542a2e49e 145 allanglemove(-1,1,1,-1);
umekou 5:2934a132d594 146 }else if(b[4]||b[6]){
umekou 5:2934a132d594 147 sa+=b[4]-b[6];
umekou 5:2934a132d594 148 if(sa==360)sa=0;
umekou 5:2934a132d594 149 frontAngle=jyroValue-sa;
umekou 5:2934a132d594 150 if(frontAngle<=-180){
umekou 5:2934a132d594 151 frontAngle+=360;
umekou 5:2934a132d594 152 }else if(frontAngle>180){
umekou 5:2934a132d594 153 frontAngle-=360;
umekou 5:2934a132d594 154 }
umekou 5:2934a132d594 155 allanglemove(0,0,0,0);
umekou 8:e1f1a91e9353 156 seki.runAll(0,0,0);
umekou 5:2934a132d594 157 }else if(b[5]){
umekou 5:2934a132d594 158 sa=0;
umekou 5:2934a132d594 159 frontAngle=jyroValue;
umekou 5:2934a132d594 160 allanglemove(0,0,0,0);
umekou 5:2934a132d594 161 }else{
umekou 5:2934a132d594 162 allanglemove(0,0,0,0);
umekou 5:2934a132d594 163 }
umekou 5:2934a132d594 164 addPID();
umekou 5:2934a132d594 165 }else if(currentMode==2){
umekou 5:2934a132d594 166 if(bc==1){
umekou 5:2934a132d594 167 currentMode=3;
umekou 7:76790bcece4b 168 seki.stopAll();
umekou 8:e1f1a91e9353 169 seki.runAll(0,0,0);
umekou 6:e7d542a2e49e 170 }else{
umekou 7:76790bcece4b 171 seki.runAll(-0.3,0.3,0.3);
umekou 5:2934a132d594 172 }
umekou 6:e7d542a2e49e 173 allanglemove(0,0,0,0);
umekou 6:e7d542a2e49e 174 addPID();
umekou 5:2934a132d594 175 }else if(currentMode==3){
umekou 5:2934a132d594 176 if(bc==1){
umekou 5:2934a132d594 177 currentMode=1;
umekou 7:76790bcece4b 178 seki.stopAll();
umekou 8:e1f1a91e9353 179 seki.runAll(0,0,0);
umekou 5:2934a132d594 180 }
umekou 8:e1f1a91e9353 181 seki.runAll(0,0,0);*/
umekou 6:e7d542a2e49e 182 allanglemove(0,0,0,0);
umekou 6:e7d542a2e49e 183 addPID();
umekou 8:e1f1a91e9353 184 // }
umekou 8:e1f1a91e9353 185
umekou 8:e1f1a91e9353 186 if(b[5]&&b[6]){
umekou 8:e1f1a91e9353 187 motorSpeed[15]=0;
umekou 8:e1f1a91e9353 188 }else if(b[5]){
umekou 8:e1f1a91e9353 189 motorSpeed[15]=0.5;
umekou 8:e1f1a91e9353 190 }else if(b[6]){
umekou 8:e1f1a91e9353 191 motorSpeed[15]=-0.5;
umekou 8:e1f1a91e9353 192 }else{
umekou 8:e1f1a91e9353 193 motorSpeed[15]=0;
umekou 8:e1f1a91e9353 194 }
umekou 8:e1f1a91e9353 195
umekou 8:e1f1a91e9353 196
umekou 8:e1f1a91e9353 197 if(b[7]==1){
umekou 8:e1f1a91e9353 198 seki.runAll(0.1,0.3,-0.3);
umekou 8:e1f1a91e9353 199 }else{
umekou 8:e1f1a91e9353 200 seki.runAll(-0.8,0.3,0.3);
umekou 6:e7d542a2e49e 201 }
umekou 6:e7d542a2e49e 202
umekou 7:76790bcece4b 203 for(int i=0; i < 4; i++){
umekou 7:76790bcece4b 204 motor[i].setSpeed(motorSpeed[i+8]*0.8);
umekou 6:e7d542a2e49e 205 }
umekou 6:e7d542a2e49e 206
umekou 8:e1f1a91e9353 207 motor[7].setSpeed(motorSpeed[15]);
umekou 8:e1f1a91e9353 208
umekou 8:e1f1a91e9353 209 pc.printf("|wheel: ");
umekou 8:e1f1a91e9353 210 for(int i=8; i < 12; i++){
umekou 8:e1f1a91e9353 211 pc.printf("%.2f ",motorSpeed[i]);
umekou 5:2934a132d594 212 }
umekou 8:e1f1a91e9353 213 pc.printf("|spin: %.2f ",motorSpeed[15]);
umekou 8:e1f1a91e9353 214 // pc.printf("|updown: %.2f ",motorSpeed[2]);
umekou 8:e1f1a91e9353 215 pc.printf("|enc: ");
umekou 8:e1f1a91e9353 216 for(int i=0; i < 7; i++){
umekou 8:e1f1a91e9353 217 pc.printf("%d ",encoderValue[i]);
umekou 8:e1f1a91e9353 218
umekou 5:2934a132d594 219 }
umekou 8:e1f1a91e9353 220 pc.printf("| ");
umekou 8:e1f1a91e9353 221 pc.printf("R1370:%.3f",jyroValue);
umekou 8:e1f1a91e9353 222 pc.printf("| ");
umekou 8:e1f1a91e9353 223 for(int i=0; i < 8; i++){
umekou 8:e1f1a91e9353 224 pc.printf("%d ",b[i]);
umekou 5:2934a132d594 225 }
umekou 8:e1f1a91e9353 226 pc.printf("| ");
umekou 8:e1f1a91e9353 227 for(int i=0; i < 4; i++){
umekou 8:e1f1a91e9353 228 pc.printf("%d ",stick[i]);
umekou 5:2934a132d594 229 }
umekou 8:e1f1a91e9353 230 pc.printf("\r\n");
ikarashikota 0:877aeb04390a 231
ikarashikota 0:877aeb04390a 232 }
ikarashikota 0:877aeb04390a 233 }