NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2022A_NHK_kikou

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

Dependencies:   OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver mbed omni_wheel

main.cpp@1:31f47d3fa8cd, 22 months ago (annotated)

Committer:
nagix
Date:
Mon Oct 10 05:43:42 2022 +0000
Revision:
1:31f47d3fa8cd
Parent:
0:b0ca7b23bdb5
Child:
2:f856cbeb5940
asimawari cant

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 1:31f47d3fa8cd 15 #define maxSpeed 0.4
nagix 0:b0ca7b23bdb5 16
nagix 0:b0ca7b23bdb5 17 DigitalIn userButton(USER_BUTTON);
nagix 0:b0ca7b23bdb5 18 AnalogIn VOLUME(A0);
nagix 0:b0ca7b23bdb5 19
nagix 0:b0ca7b23bdb5 20
nagix 0:b0ca7b23bdb5 21 FEP_RX22 mycon(fepTX, fepRX, fepad);
nagix 0:b0ca7b23bdb5 22 Serial pc(pcTX, pcRX, 115200);
nagix 0:b0ca7b23bdb5 23 Serial RS485(mdcTX,mdcRX,115200);
nagix 0:b0ca7b23bdb5 24
nagix 0:b0ca7b23bdb5 25
nagix 0:b0ca7b23bdb5 26 DigitalOut stop(em_stop);
nagix 0:b0ca7b23bdb5 27
nagix 0:b0ca7b23bdb5 28 SerialMultiByte rcv(sub2TX,sub2RX);
nagix 0:b0ca7b23bdb5 29
nagix 0:b0ca7b23bdb5 30 ikarashiMDC motor[] = {
nagix 0:b0ca7b23bdb5 31 ikarashiMDC(0,0,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 32 ikarashiMDC(0,1,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 33 ikarashiMDC(0,2,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 34 ikarashiMDC(0,3,SM,&RS485), //asi
nagix 0:b0ca7b23bdb5 35 ikarashiMDC(1,0,SM,&RS485), //全体昇降1
nagix 0:b0ca7b23bdb5 36 ikarashiMDC(1,1,SM,&RS485), //全体昇降2
nagix 1:31f47d3fa8cd 37 ikarashiMDC(1,2,SM,&RS485), //フジモン機構
nagix 1:31f47d3fa8cd 38 ikarashiMDC(1,3,SM,&RS485), //フジモン機構
nagix 1:31f47d3fa8cd 39 ikarashiMDC(2,0,SM,&RS485), //井上左昇降
nagix 1:31f47d3fa8cd 40 ikarashiMDC(2,1,SM,&RS485), //井上右昇降
nagix 1:31f47d3fa8cd 41 ikarashiMDC(2,2,SM,&RS485), //井上左前後
nagix 1:31f47d3fa8cd 42 ikarashiMDC(2,3,SM,&RS485), //井上右前後
nagix 0:b0ca7b23bdb5 43 };
nagix 0:b0ca7b23bdb5 44
nagix 0:b0ca7b23bdb5 45 Servo pwm_imput1(BLDC1);//ブラシレス宣言
nagix 0:b0ca7b23bdb5 46 Servo pwm_imput2(BLDC2);
nagix 0:b0ca7b23bdb5 47 Servo pwm_imput3(BLDC3);
nagix 0:b0ca7b23bdb5 48
nagix 1:31f47d3fa8cd 49 //abe 足回り
nagix 1:31f47d3fa8cd 50 double engine[4];
nagix 1:31f47d3fa8cd 51 double spin_power;
nagix 1:31f47d3fa8cd 52 double posiX , posiY , posiTheta;
nagix 1:31f47d3fa8cd 53 int TargetAngle = 0;
nagix 1:31f47d3fa8cd 54 int StartAngle = 0;
nagix 1:31f47d3fa8cd 55
nagix 1:31f47d3fa8cd 56 OmniWheel omni(4);
nagix 0:b0ca7b23bdb5 57 OmniPosition posi(sub1TX, sub1RX);
nagix 0:b0ca7b23bdb5 58
nagix 1:31f47d3fa8cd 59 PID angle(10.0, 5.0, 0.0000005, 0.01);
nagix 1:31f47d3fa8cd 60
nagix 1:31f47d3fa8cd 61 //プロトタイプ宣言
nagix 1:31f47d3fa8cd 62 void chassis(); //足回りの制御・ジャイロ・テラターム
nagix 1:31f47d3fa8cd 63 void spin(double ang); //PID
nagix 1:31f47d3fa8cd 64 int pm(double num); //正負判定
nagix 1:31f47d3fa8cd 65
nagix 0:b0ca7b23bdb5 66 Timer t;
nagix 0:b0ca7b23bdb5 67
nagix 1:31f47d3fa8cd 68 int16_t trigger[4];
nagix 1:31f47d3fa8cd 69 uint8_t b[16];
nagix 1:31f47d3fa8cd 70 int16_t stick[4];
nagix 1:31f47d3fa8cd 71 uint8_t data[128];
nagix 1:31f47d3fa8cd 72 int pw;
nagix 1:31f47d3fa8cd 73
nagix 1:31f47d3fa8cd 74 double speed[12] = {0};
nagix 1:31f47d3fa8cd 75
nagix 0:b0ca7b23bdb5 76 int main()
nagix 0:b0ca7b23bdb5 77 {
nagix 0:b0ca7b23bdb5 78 t.start();
nagix 1:31f47d3fa8cd 79
nagix 1:31f47d3fa8cd 80 int16_t volume[3];
nagix 1:31f47d3fa8cd 81 uint8_t toggle[4];
nagix 1:31f47d3fa8cd 82 uint8_t timeout;
nagix 0:b0ca7b23bdb5 83
nagix 1:31f47d3fa8cd 84 double bldcspeed = 0.6;
nagix 0:b0ca7b23bdb5 85
nagix 0:b0ca7b23bdb5 86 rcv.setHeaders(0xff,0xff);
nagix 0:b0ca7b23bdb5 87 rcv.startReceive(4); //SerialMultiByte受信
nagix 0:b0ca7b23bdb5 88
nagix 0:b0ca7b23bdb5 89 mycon.StartReceive(); //コントローラー受信・宣言
nagix 0:b0ca7b23bdb5 90
nagix 1:31f47d3fa8cd 91 //足回り宣言
nagix 1:31f47d3fa8cd 92 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
nagix 1:31f47d3fa8cd 93 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
nagix 1:31f47d3fa8cd 94 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
nagix 1:31f47d3fa8cd 95 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
nagix 1:31f47d3fa8cd 96
nagix 1:31f47d3fa8cd 97 angle.setInputLimits(-180,180);
nagix 1:31f47d3fa8cd 98 angle.setOutputLimits(-0.4,0.4);
nagix 1:31f47d3fa8cd 99 angle.setBias(0);
nagix 1:31f47d3fa8cd 100 angle.setMode(1);
nagix 1:31f47d3fa8cd 101 angle.setSetPoint(0);
nagix 1:31f47d3fa8cd 102
nagix 0:b0ca7b23bdb5 103
nagix 0:b0ca7b23bdb5 104 int16_t angle[4] = {0};//エンコーダ受信格納
nagix 0:b0ca7b23bdb5 105 uint8_t pulse[8] = {0};
nagix 0:b0ca7b23bdb5 106
nagix 0:b0ca7b23bdb5 107 while(1)
nagix 0:b0ca7b23bdb5 108 {
nagix 0:b0ca7b23bdb5 109 stop = toggle[0];
nagix 0:b0ca7b23bdb5 110
nagix 1:31f47d3fa8cd 111 mycon.getData(data);//コントローラー受信
nagix 0:b0ca7b23bdb5 112 for (int i=0, tmp=1; i<8; i++) {
nagix 0:b0ca7b23bdb5 113 pw = pow((float)2,i);
nagix 0:b0ca7b23bdb5 114 b[i] = (int)((data[0] & tmp)/pw);
nagix 1:31f47d3fa8cd 115 // pc.printf("%d ", b[i]);
nagix 0:b0ca7b23bdb5 116 tmp *= 2;
nagix 0:b0ca7b23bdb5 117 }
nagix 0:b0ca7b23bdb5 118 for (int i=8, tmp=1, j=0; i<16; i++, j++) {
nagix 0:b0ca7b23bdb5 119 pw = pow((float)2,j);
nagix 0:b0ca7b23bdb5 120 b[i] = (int)((data[1] & tmp)/pw);
nagix 1:31f47d3fa8cd 121 // pc.printf("%d ", b[i]);
nagix 0:b0ca7b23bdb5 122 tmp *= 2;
nagix 0:b0ca7b23bdb5 123 }
nagix 1:31f47d3fa8cd 124 // pc.printf(" | ");
nagix 0:b0ca7b23bdb5 125 for (int i=0; i<4; i++) {
nagix 0:b0ca7b23bdb5 126 stick[i] = data[i+2];
nagix 1:31f47d3fa8cd 127 // pc.printf("%3d ", stick[i]);
nagix 0:b0ca7b23bdb5 128 }
nagix 1:31f47d3fa8cd 129 // pc.printf(" | ");
nagix 0:b0ca7b23bdb5 130 for (int i=0; i<2; i++) {
nagix 0:b0ca7b23bdb5 131 trigger[i] = data[i+6];
nagix 1:31f47d3fa8cd 132 // pc.printf("%3d ", trigger[i]);
nagix 0:b0ca7b23bdb5 133 }
nagix 1:31f47d3fa8cd 134 // pc.printf(" | ");
nagix 0:b0ca7b23bdb5 135 for (int i=0; i<3; i++) {
nagix 0:b0ca7b23bdb5 136 volume[i] = data[i+9];
nagix 1:31f47d3fa8cd 137 // pc.printf("%3d ", volume[i]);
nagix 0:b0ca7b23bdb5 138 }
nagix 0:b0ca7b23bdb5 139 pc.printf(" | ");
nagix 1:31f47d3fa8cd 140 for (int i=0; i<4; i++) {
nagix 1:31f47d3fa8cd 141 toggle[i] = data[i+12];
nagix 1:31f47d3fa8cd 142 // pc.printf("%3d ", toggle[i]);
nagix 1:31f47d3fa8cd 143 }
nagix 1:31f47d3fa8cd 144 // pc.printf(" | ");
nagix 0:b0ca7b23bdb5 145 timeout = data[8];
nagix 0:b0ca7b23bdb5 146 pc.printf("%3d ", timeout);
nagix 1:31f47d3fa8cd 147 // pc.printf(" | ");
nagix 1:31f47d3fa8cd 148 if (mycon.getStatus()) pc.printf("receive");
nagix 1:31f47d3fa8cd 149 else{pc.printf("anything error...");
nagix 1:31f47d3fa8cd 150 for( int i=0; i<12; i++){
nagix 1:31f47d3fa8cd 151 motor[i].setSpeed(0);
nagix 1:31f47d3fa8cd 152 }
nagix 1:31f47d3fa8cd 153 }
nagix 0:b0ca7b23bdb5 154
nagix 0:b0ca7b23bdb5 155
nagix 1:31f47d3fa8cd 156 /*阿部君の素晴らしい天才的な足回り関数*/
nagix 1:31f47d3fa8cd 157 chassis();
nagix 1:31f47d3fa8cd 158
nagix 1:31f47d3fa8cd 159 rcv.getData(pulse); //エンコーダ受信
nagix 1:31f47d3fa8cd 160 for(int i=0,j=0;i<4;i++,j+=2){
nagix 1:31f47d3fa8cd 161 angle[i] = pulse[j] << 8 | pulse[j+1];
nagix 1:31f47d3fa8cd 162 }
nagix 1:31f47d3fa8cd 163 pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]);
nagix 1:31f47d3fa8cd 164 pc.printf("\r\n");
nagix 0:b0ca7b23bdb5 165
nagix 0:b0ca7b23bdb5 166 //ブラシレスモーター
nagix 0:b0ca7b23bdb5 167 pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1];
nagix 0:b0ca7b23bdb5 168
nagix 0:b0ca7b23bdb5 169 pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2];
nagix 0:b0ca7b23bdb5 170
nagix 0:b0ca7b23bdb5 171 pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3];
nagix 0:b0ca7b23bdb5 172 // pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n",
nagix 0:b0ca7b23bdb5 173 // ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed);
nagix 0:b0ca7b23bdb5 174
nagix 1:31f47d3fa8cd 175 /*井上機構ON,OFF*/
nagix 1:31f47d3fa8cd 176 if(toggle[1]){
nagix 1:31f47d3fa8cd 177 speed[10] = -0.9;
nagix 1:31f47d3fa8cd 178 }else{
nagix 1:31f47d3fa8cd 179 speed[10] = 0;
nagix 1:31f47d3fa8cd 180 }
nagix 1:31f47d3fa8cd 181 if(toggle[1] && b[15]){
nagix 1:31f47d3fa8cd 182 speed[10] = 0.4;
nagix 1:31f47d3fa8cd 183 }
nagix 1:31f47d3fa8cd 184
nagix 1:31f47d3fa8cd 185 if(toggle[3]){
nagix 1:31f47d3fa8cd 186 speed[11] = -0.9;
nagix 1:31f47d3fa8cd 187 }else{
nagix 1:31f47d3fa8cd 188 speed[11] = 0;
nagix 1:31f47d3fa8cd 189 }
nagix 1:31f47d3fa8cd 190 if(toggle[3] && b[15]){
nagix 1:31f47d3fa8cd 191 speed[11] = 0.4;
nagix 1:31f47d3fa8cd 192 }
nagix 1:31f47d3fa8cd 193
nagix 1:31f47d3fa8cd 194 /*フジモン機構*/
nagix 1:31f47d3fa8cd 195 if(toggle[2]){
nagix 1:31f47d3fa8cd 196 speed[6] = 0.6;
nagix 1:31f47d3fa8cd 197 speed[7] = 0.6;
nagix 1:31f47d3fa8cd 198 }else{
nagix 1:31f47d3fa8cd 199 speed[6] = 0;
nagix 1:31f47d3fa8cd 200 speed[7] = 0;
nagix 1:31f47d3fa8cd 201 }
nagix 1:31f47d3fa8cd 202
nagix 0:b0ca7b23bdb5 203 /*展開昇降*/
nagix 0:b0ca7b23bdb5 204 if(b[5] != 0){
nagix 1:31f47d3fa8cd 205 speed[4] = 0.5;
nagix 1:31f47d3fa8cd 206 speed[5] = 0.5;
nagix 0:b0ca7b23bdb5 207 }else if(b[4] != 0){
nagix 1:31f47d3fa8cd 208 speed[4] = -0.35;
nagix 1:31f47d3fa8cd 209 speed[5] = -0.35;
nagix 0:b0ca7b23bdb5 210 }else{
nagix 0:b0ca7b23bdb5 211 speed[4] = 0;
nagix 0:b0ca7b23bdb5 212 speed[5] = 0;
nagix 0:b0ca7b23bdb5 213 }
nagix 0:b0ca7b23bdb5 214 //機構昇降
nagix 1:31f47d3fa8cd 215 if(b[9]){
nagix 1:31f47d3fa8cd 216 speed[8] = -0.35;
nagix 1:31f47d3fa8cd 217 }else
nagix 1:31f47d3fa8cd 218 if(b[13]){
nagix 1:31f47d3fa8cd 219 speed[8] = 0.4;
nagix 1:31f47d3fa8cd 220 }
nagix 1:31f47d3fa8cd 221 if(b[11]){
nagix 1:31f47d3fa8cd 222 speed[9] = -0.35;
nagix 1:31f47d3fa8cd 223 }else
nagix 1:31f47d3fa8cd 224 if(b[14]){
nagix 1:31f47d3fa8cd 225 speed[9] = 0.4;
nagix 1:31f47d3fa8cd 226 }
nagix 1:31f47d3fa8cd 227 if((b[9]!=1) && (b[13]!=1)){
nagix 1:31f47d3fa8cd 228 speed[8]=0;
nagix 1:31f47d3fa8cd 229 }
nagix 1:31f47d3fa8cd 230 if((b[11]!=1) && (b[14]!=1)){
nagix 1:31f47d3fa8cd 231 speed[9]=0;
nagix 1:31f47d3fa8cd 232 }
nagix 1:31f47d3fa8cd 233 for(int i=0; i<12; i++) pc.printf("%.2f ",speed[i]);
nagix 1:31f47d3fa8cd 234 for(int i=4; i<12; i++) motor[i].setSpeed(speed[i]);
nagix 1:31f47d3fa8cd 235 }
nagix 1:31f47d3fa8cd 236
nagix 1:31f47d3fa8cd 237 }
nagix 1:31f47d3fa8cd 238
nagix 1:31f47d3fa8cd 239 void chassis(){
nagix 1:31f47d3fa8cd 240
nagix 1:31f47d3fa8cd 241 mycon.getData(data);
nagix 1:31f47d3fa8cd 242 for (int i=0; i<4; i++) {
nagix 1:31f47d3fa8cd 243 stick[i] = data[i+2];
nagix 1:31f47d3fa8cd 244 }
nagix 1:31f47d3fa8cd 245
nagix 1:31f47d3fa8cd 246 /*ジャイロ読み取り*/
nagix 1:31f47d3fa8cd 247 posiX = posi.getX();
nagix 1:31f47d3fa8cd 248 posiY = posi.getY();
nagix 1:31f47d3fa8cd 249 posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
nagix 1:31f47d3fa8cd 250 pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
nagix 1:31f47d3fa8cd 251
nagix 1:31f47d3fa8cd 252 if(abs(stick[2]) < 10){
nagix 1:31f47d3fa8cd 253 if(fabs(TargetAngle-posiTheta)>8){
nagix 1:31f47d3fa8cd 254 TargetAngle += 15*pm(posiTheta-TargetAngle);
nagix 1:31f47d3fa8cd 255 if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
nagix 1:31f47d3fa8cd 256 TargetAngle += -360*pm(TargetAngle);
nagix 1:31f47d3fa8cd 257 }
nagix 1:31f47d3fa8cd 258 }
nagix 1:31f47d3fa8cd 259 spin(TargetAngle);
nagix 1:31f47d3fa8cd 260 }
nagix 1:31f47d3fa8cd 261
nagix 1:31f47d3fa8cd 262 /*全方位*/
nagix 1:31f47d3fa8cd 263 for(int i=0; i<4; i++){
nagix 1:31f47d3fa8cd 264 if(abs(stick[i]) > 10){
nagix 1:31f47d3fa8cd 265 if(trigger[1]<10) trigger[1] = 0;
nagix 1:31f47d3fa8cd 266 engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
nagix 1:31f47d3fa8cd 267 }else if(b[i]%2){
nagix 1:31f47d3fa8cd 268 if(trigger[1]<10) trigger[1] = 0;
nagix 1:31f47d3fa8cd 269 //b[1]のとき左向き（負）b[3]のとき右向き（正）
nagix 1:31f47d3fa8cd 270 engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
nagix 1:31f47d3fa8cd 271 engine[1] = 0;
nagix 1:31f47d3fa8cd 272 }else if(b[i]%2==0){
nagix 1:31f47d3fa8cd 273 if(trigger[1]<10) trigger[1] = 0;
nagix 1:31f47d3fa8cd 274 //b[0]のとき上向き（正）b[2]のとき下向き（負）
nagix 1:31f47d3fa8cd 275 engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
nagix 1:31f47d3fa8cd 276 engine[0] = 0;
nagix 1:31f47d3fa8cd 277 }else{
nagix 1:31f47d3fa8cd 278 engine[i] = 0;
nagix 1:31f47d3fa8cd 279 }
nagix 1:31f47d3fa8cd 280 }
nagix 1:31f47d3fa8cd 281 /*旋回*/
nagix 1:31f47d3fa8cd 282 if(abs(stick[2]) > 10){
nagix 1:31f47d3fa8cd 283 spin_power = engine[2];
nagix 0:b0ca7b23bdb5 284 }else{
nagix 1:31f47d3fa8cd 285 spin_power = angle.compute();
nagix 0:b0ca7b23bdb5 286 }
nagix 0:b0ca7b23bdb5 287
nagix 1:31f47d3fa8cd 288 omni.computeXY(engine[0],engine[1],-spin_power);
nagix 1:31f47d3fa8cd 289 for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
nagix 1:31f47d3fa8cd 290 for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
nagix 0:b0ca7b23bdb5 291
nagix 1:31f47d3fa8cd 292 }
nagix 1:31f47d3fa8cd 293
nagix 1:31f47d3fa8cd 294
nagix 1:31f47d3fa8cd 295 void spin(double ang)
nagix 1:31f47d3fa8cd 296 {
nagix 1:31f47d3fa8cd 297 angle.setSetPoint(ang);
nagix 1:31f47d3fa8cd 298 posiTheta = posi.getTheta()*(180.0/M_PI);
nagix 1:31f47d3fa8cd 299 angle.setProcessValue(posiTheta);
nagix 1:31f47d3fa8cd 300 pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
nagix 1:31f47d3fa8cd 301 //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
nagix 1:31f47d3fa8cd 302 }
nagix 1:31f47d3fa8cd 303
nagix 1:31f47d3fa8cd 304
nagix 1:31f47d3fa8cd 305 int pm(double num){
nagix 1:31f47d3fa8cd 306 return((num>0)-(num<0));
nagix 1:31f47d3fa8cd 307 }