NagaokaRoboticsClub_mbedTeam / Mbed OS 2022NHK_A_main

NHK2022Aチームの足回りと機構のセット　メインプログラム

Dependencies:   FEP_RX22 OmniPosition PID R1370 ikarashiMDC_2byte_ver omni_wheel SerialMultiByte Servo

main.cpp@9:b0e2dd478db7, 23 months ago (annotated)

Committer:
nagix
Date:
Wed Oct 12 00:32:44 2022 +0000
Revision:
9:b0e2dd478db7
Parent:
8:f40325f2d182
Child:
10:533014972963
asi

Who changed what in which revision?

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