abe takumi / Mbed OS 2022NHK_A_main

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

Dependencies:   FEP_RX22 OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver omni_wheel

main.cpp@6:d4b82ba4836a, 2022-10-09 (annotated)

Committer:
me33004m
Date:
Sun Oct 09 10:17:42 2022 +0000
Revision:
6:d4b82ba4836a
Parent:
5:885bffdceaa2
Child:
7:778eaeae8128
probably could

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 2:d84346eaa720 3 #include "FEP_RX22.h"
me33004m 0:5d4705b2893c 4 #include "pinconfig.h"
me33004m 0:5d4705b2893c 5 #include "omni_wheel.h"
me33004m 0:5d4705b2893c 6 #include "math.h"
me33004m 0:5d4705b2893c 7 #include "OmniPosition.h"
me33004m 0:5d4705b2893c 8 #include "PID.h"
me33004m 1:5132f966db08 9
me33004m 0:5d4705b2893c 10 #define PI 3.141592653589
me33004m 3:4c0c8046c3a7 11 #define maxSpeed 0.4
me33004m 1:5132f966db08 12
me33004m 2:d84346eaa720 13 FEP_RX22 mycon(fepTX, fepRX, fepad);
me33004m 0:5d4705b2893c 14 Serial pc(pcTX, pcRX, 115200);
me33004m 0:5d4705b2893c 15 Serial RS485(PC_10,PC_11,115200);
me33004m 1:5132f966db08 16
me33004m 1:5132f966db08 17
me33004m 3:4c0c8046c3a7 18 uint8_t b[16];
me33004m 0:5d4705b2893c 19 int16_t stick[4];
me33004m 2:d84346eaa720 20 int16_t trigger[4];
me33004m 6:d4b82ba4836a 21 int16_t volume[3];
me33004m 6:d4b82ba4836a 22 uint8_t toggle[4];
me33004m 6:d4b82ba4836a 23 uint8_t timeout;
me33004m 6:d4b82ba4836a 24 uint8_t data[128];
me33004m 6:d4b82ba4836a 25 int pw;
me33004m 0:5d4705b2893c 26 double engine[4];
me33004m 0:5d4705b2893c 27 double speed[6];
me33004m 0:5d4705b2893c 28 double spin_power;
me33004m 0:5d4705b2893c 29 double posiX , posiY , posiTheta;
me33004m 6:d4b82ba4836a 30 int TargetAngle = 0;
me33004m 6:d4b82ba4836a 31 int StartAngle = 0;
me33004m 6:d4b82ba4836a 32 DigitalOut stop(PA_5);
me33004m 0:5d4705b2893c 33 ikarashiMDC motor[] = {
me33004m 0:5d4705b2893c 34 ikarashiMDC(0,0,SM,&RS485),
me33004m 0:5d4705b2893c 35 ikarashiMDC(0,1,SM,&RS485),
me33004m 0:5d4705b2893c 36 ikarashiMDC(0,2,SM,&RS485),
me33004m 0:5d4705b2893c 37 ikarashiMDC(0,3,SM,&RS485),
me33004m 0:5d4705b2893c 38 };
me33004m 1:5132f966db08 39
me33004m 0:5d4705b2893c 40 OmniWheel omni(4);
me33004m 0:5d4705b2893c 41 OmniPosition posi(mwTX, mwRX);
me33004m 1:5132f966db08 42
me33004m 0:5d4705b2893c 43 PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！
me33004m 1:5132f966db08 44
me33004m 0:5d4705b2893c 45 /*プロトタイプ宣言*/
me33004m 3:4c0c8046c3a7 46 void chassis(); //足回りの制御・ジャイロ・テラターム
me33004m 3:4c0c8046c3a7 47 void spin(double ang); //PID
me33004m 3:4c0c8046c3a7 48 int pm(double num); //正負判定
me33004m 1:5132f966db08 49
me33004m 0:5d4705b2893c 50 int main(void){
me33004m 0:5d4705b2893c 51 mycon.StartReceive();
me33004m 1:5132f966db08 52
me33004m 0:5d4705b2893c 53 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
me33004m 0:5d4705b2893c 54 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
me33004m 0:5d4705b2893c 55 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
me33004m 0:5d4705b2893c 56 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
me33004m 0:5d4705b2893c 57
me33004m 0:5d4705b2893c 58 angle.setInputLimits(-180,180);
me33004m 0:5d4705b2893c 59 angle.setOutputLimits(-0.4,0.4);
me33004m 0:5d4705b2893c 60 angle.setBias(0);
me33004m 0:5d4705b2893c 61 angle.setMode(1);
me33004m 0:5d4705b2893c 62 angle.setSetPoint(0);
me33004m 0:5d4705b2893c 63 while(1){
me33004m 6:d4b82ba4836a 64 stop = toggle[0];
me33004m 0:5d4705b2893c 65 chassis();
me33004m 0:5d4705b2893c 66 }
me33004m 0:5d4705b2893c 67
me33004m 0:5d4705b2893c 68 }
me33004m 1:5132f966db08 69
me33004m 0:5d4705b2893c 70 void chassis(){
me33004m 3:4c0c8046c3a7 71
me33004m 1:5132f966db08 72
me33004m 0:5d4705b2893c 73 /*ジャイロ読み取り*/
me33004m 0:5d4705b2893c 74 posiX = posi.getX();
me33004m 0:5d4705b2893c 75 posiY = posi.getY();
me33004m 0:5d4705b2893c 76 posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
me33004m 0:5d4705b2893c 77 pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
me33004m 0:5d4705b2893c 78 /*コントローラー受信*/
me33004m 6:d4b82ba4836a 79 mycon.getData(data);
me33004m 6:d4b82ba4836a 80 for (int i=0, tmp=1; i<8; i++) {
me33004m 6:d4b82ba4836a 81 pw = pow((float)2,i);
me33004m 6:d4b82ba4836a 82 b[i] = (int)((data[0] & tmp)/pw);
me33004m 6:d4b82ba4836a 83 pc.printf("%d ", b[i]);
me33004m 6:d4b82ba4836a 84 tmp *= 2;
me33004m 6:d4b82ba4836a 85 }
me33004m 6:d4b82ba4836a 86 for (int i=8, tmp=1, j=0; i<16; i++, j++) {
me33004m 6:d4b82ba4836a 87 pw = pow((float)2,j);
me33004m 6:d4b82ba4836a 88 b[i] = (int)((data[1] & tmp)/pw);
me33004m 6:d4b82ba4836a 89 pc.printf("%d ", b[i]);
me33004m 6:d4b82ba4836a 90 tmp *= 2;
me33004m 6:d4b82ba4836a 91 }
me33004m 6:d4b82ba4836a 92 pc.printf(" | ");
me33004m 6:d4b82ba4836a 93
me33004m 6:d4b82ba4836a 94 for (int i=0; i<4; i++) {
me33004m 6:d4b82ba4836a 95 stick[i] = data[i+2];
me33004m 6:d4b82ba4836a 96 pc.printf("%3d ", stick[i]);
me33004m 2:d84346eaa720 97 }
me33004m 6:d4b82ba4836a 98 pc.printf(" | ");
me33004m 6:d4b82ba4836a 99
me33004m 6:d4b82ba4836a 100 for (int i=0; i<2; i++) {
me33004m 6:d4b82ba4836a 101 trigger[i] = data[i+6];
me33004m 6:d4b82ba4836a 102 pc.printf("%3d ", trigger[i]);
me33004m 6:d4b82ba4836a 103 }
me33004m 0:5d4705b2893c 104 pc.printf(" | ");
me33004m 6:d4b82ba4836a 105
me33004m 6:d4b82ba4836a 106 for (int i=0; i<3; i++) {
me33004m 6:d4b82ba4836a 107 volume[i] = data[i+9];
me33004m 6:d4b82ba4836a 108 pc.printf("%3d ", volume[i]);
me33004m 6:d4b82ba4836a 109 }
me33004m 0:5d4705b2893c 110 pc.printf(" | ");
me33004m 6:d4b82ba4836a 111
me33004m 6:d4b82ba4836a 112 for (int i=0; i<4; i++) {
me33004m 6:d4b82ba4836a 113 toggle[i] = data[i+12];
me33004m 6:d4b82ba4836a 114 pc.printf("%d ", toggle[i]);
me33004m 6:d4b82ba4836a 115 }
me33004m 6:d4b82ba4836a 116 pc.printf(" | ");
me33004m 6:d4b82ba4836a 117
me33004m 6:d4b82ba4836a 118 timeout = data[8];
me33004m 6:d4b82ba4836a 119 pc.printf("%3d ", timeout);
me33004m 6:d4b82ba4836a 120 pc.printf(" | ");
me33004m 6:d4b82ba4836a 121
me33004m 2:d84346eaa720 122 if(mycon.getStatus()) pc.printf("received\r\n");
me33004m 1:5132f966db08 123 else{ pc.printf("anything error...\r\n");
me33004m 1:5132f966db08 124 for( int i=0; i<4; i++){
me33004m 1:5132f966db08 125 motor[i].setSpeed(0);
me33004m 1:5132f966db08 126 }
me33004m 0:5d4705b2893c 127 }
me33004m 6:d4b82ba4836a 128 for(int i=0;i<4;i++){
me33004m 6:d4b82ba4836a 129 stick[i] = stick[i] - 128;
me33004m 0:5d4705b2893c 130 }
me33004m 6:d4b82ba4836a 131
me33004m 0:5d4705b2893c 132 /*PID*/
me33004m 0:5d4705b2893c 133 /*スパゲッティコードで申し訳ないです...*/
me33004m 2:d84346eaa720 134 if(abs(stick[2]) < 10){/*
me33004m 0:5d4705b2893c 135 if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){
me33004m 0:5d4705b2893c 136 spin(0);
me33004m 1:5132f966db08 137 pc.printf("a");
me33004m 0:5d4705b2893c 138 }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){
me33004m 0:5d4705b2893c 139 spin(15);
me33004m 1:5132f966db08 140 pc.printf("b");
me33004m 0:5d4705b2893c 141 }else if(((posiTheta>=22.5)&&(posiTheta<37.5))&&((posiTheta>31)||(posiTheta<29))){
me33004m 0:5d4705b2893c 142 spin(30);
me33004m 0:5d4705b2893c 143 }else if(((posiTheta>=37.5)&&(posiTheta<52.5))&&((posiTheta>46)||(posiTheta<44))){
me33004m 0:5d4705b2893c 144 spin(45);
me33004m 0:5d4705b2893c 145 }else if(((posiTheta>=52.5)&&(posiTheta<67.5))&&((posiTheta>61)||(posiTheta<59))){
me33004m 0:5d4705b2893c 146 spin(60);
me33004m 0:5d4705b2893c 147 }else if(((posiTheta>=67.5)&&(posiTheta<82.5))&&((posiTheta>76)||(posiTheta<74))){
me33004m 0:5d4705b2893c 148 spin(75);
me33004m 0:5d4705b2893c 149 }else if(((posiTheta>=82.5)&&(posiTheta<97.5))&&((posiTheta>91)||(posiTheta<89))){
me33004m 0:5d4705b2893c 150 spin(90);
me33004m 0:5d4705b2893c 151 }else if(((posiTheta>=97.5)&&(posiTheta<112.5))&&((posiTheta>106)||(posiTheta<104))){
me33004m 0:5d4705b2893c 152 spin(105);
me33004m 0:5d4705b2893c 153 }else if(((posiTheta>=112.5)&&(posiTheta<127.5))&&((posiTheta>121)||(posiTheta<119))){
me33004m 0:5d4705b2893c 154 spin(120);
me33004m 0:5d4705b2893c 155 }else if(((posiTheta>=127.5)&&(posiTheta<142.5))&&((posiTheta>136)||(posiTheta<134))){
me33004m 0:5d4705b2893c 156 spin(135);
me33004m 0:5d4705b2893c 157 }else if(((posiTheta>=142.5)&&(posiTheta<157.5))&&((posiTheta>151)||(posiTheta<149))){
me33004m 0:5d4705b2893c 158 spin(150);
me33004m 0:5d4705b2893c 159 }else if(((posiTheta>=157.5)&&(posiTheta<172.5))&&((posiTheta>166)||(posiTheta<164))){
me33004m 0:5d4705b2893c 160 spin(165);
me33004m 0:5d4705b2893c 161 }else if(((posiTheta>=172.5)&&(posiTheta<=179))||((posiTheta<=-172.5)&&(posiTheta>=-179))){
me33004m 0:5d4705b2893c 162 spin(180);
me33004m 0:5d4705b2893c 163 }else if(((posiTheta<=-157.5)&&(posiTheta>-172.5))&&((posiTheta>-164)||(posiTheta<-166))){
me33004m 0:5d4705b2893c 164 spin(-165);
me33004m 0:5d4705b2893c 165 }else if(((posiTheta<=-142.5)&&(posiTheta>-157.5))&&((posiTheta>-149)||(posiTheta<-151))){
me33004m 0:5d4705b2893c 166 spin(-150);
me33004m 0:5d4705b2893c 167 }else if(((posiTheta<=-127.5)&&(posiTheta>-142.5))&&((posiTheta>-134)||(posiTheta<-136))){
me33004m 0:5d4705b2893c 168 spin(-135);
me33004m 0:5d4705b2893c 169 }else if(((posiTheta<=-112.5)&&(posiTheta>-127.5))&&((posiTheta>-119)||(posiTheta<-121))){
me33004m 0:5d4705b2893c 170 spin(-120);
me33004m 0:5d4705b2893c 171 }else if(((posiTheta<=-97.5)&&(posiTheta>-112.5))&&((posiTheta>-104)||(posiTheta<-106))){
me33004m 0:5d4705b2893c 172 spin(-105);
me33004m 0:5d4705b2893c 173 }else if(((posiTheta<=-82.5)&&(posiTheta>-90.5))&&((posiTheta>-89)||(posiTheta<-91))){
me33004m 0:5d4705b2893c 174 spin(-90);
me33004m 0:5d4705b2893c 175 }else if(((posiTheta<=-67.5)&&(posiTheta>-82.5))&&((posiTheta>-74)||(posiTheta<-76))){
me33004m 0:5d4705b2893c 176 spin(-75);
me33004m 0:5d4705b2893c 177 }else if(((posiTheta<=-52.5)&&(posiTheta>-67.5))&&((posiTheta>-59)||(posiTheta<-61))){
me33004m 0:5d4705b2893c 178 spin(-60);
me33004m 0:5d4705b2893c 179 }else if(((posiTheta<=-37.5)&&(posiTheta>-52.5))&&((posiTheta>-44)||(posiTheta<-46))){
me33004m 0:5d4705b2893c 180 spin(-45);
me33004m 0:5d4705b2893c 181 }else if(((posiTheta<=-22.5)&&(posiTheta>-37.5))&&((posiTheta>-29)||(posiTheta<-30))){
me33004m 0:5d4705b2893c 182 spin(-30);
me33004m 0:5d4705b2893c 183 }else if(((posiTheta<=-7.5)&&(posiTheta>-22.5))&&((posiTheta>-14)||(posiTheta<-16))){
me33004m 0:5d4705b2893c 184 spin(-15);
me33004m 0:5d4705b2893c 185 }else{
me33004m 2:d84346eaa720 186 }*/
me33004m 2:d84346eaa720 187 /*for(int i=-180;i<=180;i+=15){
me33004m 2:d84346eaa720 188 if(((posiTheta>=i-7.5)&&(posiTheta<i+7.5))&&((posiTheta>i+1)||(posiTheta<i-1))){
me33004m 2:d84346eaa720 189 spin(i);
me33004m 2:d84346eaa720 190 }
me33004m 2:d84346eaa720 191 else if((posiTheta == i) ||(posiTheta == i+1) ||(posiTheta == i-1)){
me33004m 2:d84346eaa720 192 }
me33004m 2:d84346eaa720 193 }*/
me33004m 6:d4b82ba4836a 194 // if(fabs(posiTheta)<8){
me33004m 6:d4b82ba4836a 195 // spin(StartAngle);
me33004m 6:d4b82ba4836a 196 // }
me33004m 6:d4b82ba4836a 197 // if(fabs(TargetAngle-posiTheta)<8){
me33004m 6:d4b82ba4836a 198 // spin(TargetAngle);
me33004m 6:d4b82ba4836a 199 // }
me33004m 2:d84346eaa720 200 if(fabs(TargetAngle-posiTheta)>8){
me33004m 2:d84346eaa720 201 TargetAngle += 15*pm(posiTheta-TargetAngle);
me33004m 5:885bffdceaa2 202 if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。
me33004m 2:d84346eaa720 203 TargetAngle += -360*pm(TargetAngle);
me33004m 2:d84346eaa720 204 }
me33004m 2:d84346eaa720 205 }
me33004m 6:d4b82ba4836a 206 spin(TargetAngle);
me33004m 0:5d4705b2893c 207 }
me33004m 6:d4b82ba4836a 208 /*全方位*/
me33004m 6:d4b82ba4836a 209 for(int i=0; i<4; i++){
me33004m 6:d4b82ba4836a 210 if(abs(stick[i]) > 10){
me33004m 6:d4b82ba4836a 211 if(trigger[1]<10) trigger[1] = 0;
me33004m 6:d4b82ba4836a 212 engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0);
me33004m 6:d4b82ba4836a 213 }else if(b[i]%2){
me33004m 6:d4b82ba4836a 214 if(trigger[1]<10) trigger[1] = 0;
me33004m 6:d4b82ba4836a 215 //b[1]のとき左向き（負）b[3]のとき右向き（正）
me33004m 6:d4b82ba4836a 216 engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0);
me33004m 6:d4b82ba4836a 217 engine[1] = 0;
me33004m 6:d4b82ba4836a 218 }else if(b[i]%2==0){
me33004m 6:d4b82ba4836a 219 if(trigger[1]<10) trigger[1] = 0;
me33004m 6:d4b82ba4836a 220 //b[0]のとき上向き（正）b[2]のとき下向き（負）
me33004m 6:d4b82ba4836a 221 engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0);
me33004m 6:d4b82ba4836a 222 engine[0] = 0;
me33004m 6:d4b82ba4836a 223 }else{
me33004m 6:d4b82ba4836a 224 engine[i] = 0;
me33004m 6:d4b82ba4836a 225 }
me33004m 6:d4b82ba4836a 226 }
me33004m 1:5132f966db08 227 /*旋回*/
me33004m 1:5132f966db08 228 if(abs(stick[2]) > 10){
me33004m 1:5132f966db08 229 spin_power = engine[2];
me33004m 1:5132f966db08 230 }else{
me33004m 1:5132f966db08 231 spin_power = angle.compute();
me33004m 1:5132f966db08 232 }
me33004m 1:5132f966db08 233
me33004m 1:5132f966db08 234 omni.computeXY(engine[0],engine[1],-spin_power);
me33004m 1:5132f966db08 235 for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
me33004m 1:5132f966db08 236
me33004m 1:5132f966db08 237
me33004m 1:5132f966db08 238 for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
me33004m 1:5132f966db08 239
me33004m 0:5d4705b2893c 240 }
me33004m 0:5d4705b2893c 241 void spin(double ang)
me33004m 0:5d4705b2893c 242 {
me33004m 0:5d4705b2893c 243 angle.setSetPoint(ang);
me33004m 2:d84346eaa720 244 posiTheta = posi.getTheta()*(180.0/M_PI);
me33004m 2:d84346eaa720 245 angle.setProcessValue(posiTheta);
me33004m 6:d4b82ba4836a 246 pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta);
me33004m 2:d84346eaa720 247 //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
me33004m 2:d84346eaa720 248 }
me33004m 2:d84346eaa720 249 int pm(double num){
me33004m 2:d84346eaa720 250 return((num>0)-(num<0));
me33004m 0:5d4705b2893c 251 }