NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2022A_NHK_kikou

2022NHKAチーム(射出、紙飛行機折り、昇降)

Dependencies:   OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver mbed omni_wheel

main.cpp@0:b0ca7b23bdb5, 22 months ago (annotated)

Committer:
nagix
Date:
Sun Oct 09 08:59:22 2022 +0000
Revision:
0:b0ca7b23bdb5
Child:
1:31f47d3fa8cd
2022Amain

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nagix 0:b0ca7b23bdb5 1 #include "mbed.h"
nagix 0:b0ca7b23bdb5 2 #include "ikarashiMDC.h"
nagix 0:b0ca7b23bdb5 3 #include "pinconfig.h"
nagix 0:b0ca7b23bdb5 4 #include "omni_wheel.h"
nagix 0:b0ca7b23bdb5 5 #include "Servo.h"
nagix 0:b0ca7b23bdb5 6 #include "math.h"
nagix 0:b0ca7b23bdb5 7 #include "SerialMultiByte.h"
nagix 0:b0ca7b23bdb5 8 #include "PID.h"
nagix 0:b0ca7b23bdb5 9 #include "R1370.h"
nagix 0:b0ca7b23bdb5 10 #include "OmniPosition.h"
nagix 0:b0ca7b23bdb5 11 #include "FEP_RX22.h"
nagix 0:b0ca7b23bdb5 12 #include "cmath"
nagix 0:b0ca7b23bdb5 13
nagix 0:b0ca7b23bdb5 14 #define PI 3.141592653589
nagix 0:b0ca7b23bdb5 15
nagix 0:b0ca7b23bdb5 16 DigitalIn userButton(USER_BUTTON);
nagix 0:b0ca7b23bdb5 17 AnalogIn VOLUME(A0);
nagix 0:b0ca7b23bdb5 18
nagix 0:b0ca7b23bdb5 19
nagix 0:b0ca7b23bdb5 20 FEP_RX22 mycon(fepTX, fepRX, fepad);
nagix 0:b0ca7b23bdb5 21 Serial pc(pcTX, pcRX, 115200);
nagix 0:b0ca7b23bdb5 22 Serial RS485(mdcTX,mdcRX,115200);
nagix 0:b0ca7b23bdb5 23
nagix 0:b0ca7b23bdb5 24
nagix 0:b0ca7b23bdb5 25 DigitalOut stop(em_stop);
nagix 0:b0ca7b23bdb5 26
nagix 0:b0ca7b23bdb5 27 SerialMultiByte rcv(sub2TX,sub2RX);
nagix 0:b0ca7b23bdb5 28
nagix 0:b0ca7b23bdb5 29 ikarashiMDC motor[] = {
nagix 0:b0ca7b23bdb5 30 ikarashiMDC(0,0,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 31 ikarashiMDC(0,1,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 32 ikarashiMDC(0,2,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 33 ikarashiMDC(0,3,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 34 ikarashiMDC(1,0,SM,&RS485), //全体昇降1
nagix 0:b0ca7b23bdb5 35 ikarashiMDC(1,1,SM,&RS485), //全体昇降2
nagix 0:b0ca7b23bdb5 36 ikarashiMDC(1,2,SM,&RS485), //井上左昇降
nagix 0:b0ca7b23bdb5 37 ikarashiMDC(1,3,SM,&RS485), //井上右昇降
nagix 0:b0ca7b23bdb5 38 ikarashiMDC(2,0,SM,&RS485),
nagix 0:b0ca7b23bdb5 39 ikarashiMDC(2,1,SM,&RS485),
nagix 0:b0ca7b23bdb5 40 ikarashiMDC(2,2,SM,&RS485),
nagix 0:b0ca7b23bdb5 41 ikarashiMDC(2,3,SM,&RS485),
nagix 0:b0ca7b23bdb5 42 };
nagix 0:b0ca7b23bdb5 43
nagix 0:b0ca7b23bdb5 44 Servo pwm_imput1(BLDC1);//ブラシレス宣言
nagix 0:b0ca7b23bdb5 45 Servo pwm_imput2(BLDC2);
nagix 0:b0ca7b23bdb5 46 Servo pwm_imput3(BLDC3);
nagix 0:b0ca7b23bdb5 47
nagix 0:b0ca7b23bdb5 48 OmniWheel omni(4);//足回り宣言
nagix 0:b0ca7b23bdb5 49 OmniPosition posi(sub1TX, sub1RX);
nagix 0:b0ca7b23bdb5 50
nagix 0:b0ca7b23bdb5 51 Timer t;
nagix 0:b0ca7b23bdb5 52
nagix 0:b0ca7b23bdb5 53 int main()
nagix 0:b0ca7b23bdb5 54 {
nagix 0:b0ca7b23bdb5 55 t.start();
nagix 0:b0ca7b23bdb5 56
nagix 0:b0ca7b23bdb5 57 double bldcspeed = 0.8;
nagix 0:b0ca7b23bdb5 58
nagix 0:b0ca7b23bdb5 59 // double spin_power; //足回り宣言
nagix 0:b0ca7b23bdb5 60 // double posiX , posiY , posiTheta;
nagix 0:b0ca7b23bdb5 61
nagix 0:b0ca7b23bdb5 62 rcv.setHeaders(0xff,0xff);
nagix 0:b0ca7b23bdb5 63 rcv.startReceive(4); //SerialMultiByte受信
nagix 0:b0ca7b23bdb5 64
nagix 0:b0ca7b23bdb5 65 mycon.StartReceive(); //コントローラー受信・宣言
nagix 0:b0ca7b23bdb5 66 uint8_t b[16];
nagix 0:b0ca7b23bdb5 67 int16_t stick[4];
nagix 0:b0ca7b23bdb5 68 int16_t trigger[4];
nagix 0:b0ca7b23bdb5 69 int16_t volume[3];
nagix 0:b0ca7b23bdb5 70 uint8_t toggle[4];
nagix 0:b0ca7b23bdb5 71 uint8_t timeout;
nagix 0:b0ca7b23bdb5 72 uint8_t data[128];
nagix 0:b0ca7b23bdb5 73 int pw;
nagix 0:b0ca7b23bdb5 74
nagix 0:b0ca7b23bdb5 75 double speed[12] = {0};
nagix 0:b0ca7b23bdb5 76
nagix 0:b0ca7b23bdb5 77 int16_t angle[4] = {0};//エンコーダ受信格納
nagix 0:b0ca7b23bdb5 78 uint8_t pulse[8] = {0};
nagix 0:b0ca7b23bdb5 79
nagix 0:b0ca7b23bdb5 80 while(1)
nagix 0:b0ca7b23bdb5 81 {
nagix 0:b0ca7b23bdb5 82 stop = toggle[0];
nagix 0:b0ca7b23bdb5 83
nagix 0:b0ca7b23bdb5 84 rcv.getData(pulse); //エンコーダ受信
nagix 0:b0ca7b23bdb5 85 for(int i=0,j=0;i<4;i++,j+=2){
nagix 0:b0ca7b23bdb5 86 angle[i] = pulse[j] << 8 + pulse[j+1];
nagix 0:b0ca7b23bdb5 87 }
nagix 0:b0ca7b23bdb5 88 pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);
nagix 0:b0ca7b23bdb5 89 pc.printf("\r\n");
nagix 0:b0ca7b23bdb5 90
nagix 0:b0ca7b23bdb5 91 #if ControllerMode
nagix 0:b0ca7b23bdb5 92 for (int i=0; i<16; i++) b[i] = mycon.getButton(i);
nagix 0:b0ca7b23bdb5 93 for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
nagix 0:b0ca7b23bdb5 94 for (int i=0; i<2; i++) trigger[i] = mycon.getTrigger(i);
nagix 0:b0ca7b23bdb5 95
nagix 0:b0ca7b23bdb5 96 // for (int i=0; i<16; i++) pc.printf("%d ", b[i]);
nagix 0:b0ca7b23bdb5 97 // pc.printf(" | ");
nagix 0:b0ca7b23bdb5 98 // for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
nagix 0:b0ca7b23bdb5 99 // pc.printf(" | ");
nagix 0:b0ca7b23bdb5 100 // for (int i=0; i<2; i++) pc.printf("%3d ", trigger[i]);
nagix 0:b0ca7b23bdb5 101 // pc.printf(" | ");
nagix 0:b0ca7b23bdb5 102 #else
nagix 0:b0ca7b23bdb5 103 mycon.getData(data);
nagix 0:b0ca7b23bdb5 104 for (int i=0, tmp=1; i<8; i++) {
nagix 0:b0ca7b23bdb5 105 pw = pow((float)2,i);
nagix 0:b0ca7b23bdb5 106 b[i] = (int)((data[0] & tmp)/pw);
nagix 0:b0ca7b23bdb5 107 pc.printf("%d ", b[i]);
nagix 0:b0ca7b23bdb5 108 tmp *= 2;
nagix 0:b0ca7b23bdb5 109 }
nagix 0:b0ca7b23bdb5 110 for (int i=8, tmp=1, j=0; i<16; i++, j++) {
nagix 0:b0ca7b23bdb5 111 pw = pow((float)2,j);
nagix 0:b0ca7b23bdb5 112 b[i] = (int)((data[1] & tmp)/pw);
nagix 0:b0ca7b23bdb5 113 pc.printf("%d ", b[i]);
nagix 0:b0ca7b23bdb5 114 tmp *= 2;
nagix 0:b0ca7b23bdb5 115 }
nagix 0:b0ca7b23bdb5 116 pc.printf(" | ");
nagix 0:b0ca7b23bdb5 117
nagix 0:b0ca7b23bdb5 118 for (int i=0; i<4; i++) {
nagix 0:b0ca7b23bdb5 119 stick[i] = data[i+2];
nagix 0:b0ca7b23bdb5 120 pc.printf("%3d ", stick[i]);
nagix 0:b0ca7b23bdb5 121 }
nagix 0:b0ca7b23bdb5 122 pc.printf(" | ");
nagix 0:b0ca7b23bdb5 123
nagix 0:b0ca7b23bdb5 124 for (int i=0; i<2; i++) {
nagix 0:b0ca7b23bdb5 125 trigger[i] = data[i+6];
nagix 0:b0ca7b23bdb5 126 pc.printf("%3d ", trigger[i]);
nagix 0:b0ca7b23bdb5 127 }
nagix 0:b0ca7b23bdb5 128 pc.printf(" | ");
nagix 0:b0ca7b23bdb5 129
nagix 0:b0ca7b23bdb5 130 for (int i=0; i<3; i++) {
nagix 0:b0ca7b23bdb5 131 volume[i] = data[i+9];
nagix 0:b0ca7b23bdb5 132 pc.printf("%3d ", volume[i]);
nagix 0:b0ca7b23bdb5 133 }
nagix 0:b0ca7b23bdb5 134 pc.printf(" | ");
nagix 0:b0ca7b23bdb5 135
nagix 0:b0ca7b23bdb5 136 for (int i=0; i<4; i++) {
nagix 0:b0ca7b23bdb5 137 toggle[i] = data[i+12];
nagix 0:b0ca7b23bdb5 138 pc.printf("%3d ", toggle[i]);
nagix 0:b0ca7b23bdb5 139 }
nagix 0:b0ca7b23bdb5 140 pc.printf(" | ");
nagix 0:b0ca7b23bdb5 141
nagix 0:b0ca7b23bdb5 142 timeout = data[8];
nagix 0:b0ca7b23bdb5 143 pc.printf("%3d ", timeout);
nagix 0:b0ca7b23bdb5 144 pc.printf(" | ");
nagix 0:b0ca7b23bdb5 145
nagix 0:b0ca7b23bdb5 146
nagix 0:b0ca7b23bdb5 147 #endif
nagix 0:b0ca7b23bdb5 148 if (mycon.getStatus()) pc.printf("receive");
nagix 0:b0ca7b23bdb5 149 else pc.printf("anything error...");
nagix 0:b0ca7b23bdb5 150
nagix 0:b0ca7b23bdb5 151
nagix 0:b0ca7b23bdb5 152 //ブラシレスモーター
nagix 0:b0ca7b23bdb5 153 pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
nagix 0:b0ca7b23bdb5 154
nagix 0:b0ca7b23bdb5 155 pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2];
nagix 0:b0ca7b23bdb5 156
nagix 0:b0ca7b23bdb5 157 pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3];
nagix 0:b0ca7b23bdb5 158 // pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
nagix 0:b0ca7b23bdb5 159 // ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
nagix 0:b0ca7b23bdb5 160
nagix 0:b0ca7b23bdb5 161 /*展開昇降*/
nagix 0:b0ca7b23bdb5 162 if(b[5] != 0){
nagix 0:b0ca7b23bdb5 163 speed[4] = 0.3;
nagix 0:b0ca7b23bdb5 164 speed[5] = 0.3;
nagix 0:b0ca7b23bdb5 165 }else if(b[4] != 0){
nagix 0:b0ca7b23bdb5 166 speed[4] = -0.3;
nagix 0:b0ca7b23bdb5 167 speed[5] = -0.3;
nagix 0:b0ca7b23bdb5 168 }else{
nagix 0:b0ca7b23bdb5 169 speed[4] = 0;
nagix 0:b0ca7b23bdb5 170 speed[5] = 0;
nagix 0:b0ca7b23bdb5 171 }
nagix 0:b0ca7b23bdb5 172 //機構昇降
nagix 0:b0ca7b23bdb5 173 if(b[9] != 0){
nagix 0:b0ca7b23bdb5 174 speed[6] = 0.3;
nagix 0:b0ca7b23bdb5 175 }else if(b[11] != 0){
nagix 0:b0ca7b23bdb5 176 speed[7] = 0.3;
nagix 0:b0ca7b23bdb5 177 }else if(b[13] != 0){
nagix 0:b0ca7b23bdb5 178 speed[6] = -0.3;
nagix 0:b0ca7b23bdb5 179 }else if(b[14] != 0){
nagix 0:b0ca7b23bdb5 180 speed[7] = -0.3;
nagix 0:b0ca7b23bdb5 181 }else{
nagix 0:b0ca7b23bdb5 182 speed[6] = 0;
nagix 0:b0ca7b23bdb5 183 speed[7] = 0;
nagix 0:b0ca7b23bdb5 184 }
nagix 0:b0ca7b23bdb5 185
nagix 0:b0ca7b23bdb5 186 for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]);
nagix 0:b0ca7b23bdb5 187
nagix 0:b0ca7b23bdb5 188
nagix 0:b0ca7b23bdb5 189 }
nagix 0:b0ca7b23bdb5 190
nagix 0:b0ca7b23bdb5 191 }