![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |