NagaokaRoboticsClub_mbedTeam / Mbed OS 2022NHK_A_main

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

Dependencies:   FEP_RX22 OmniPosition PID R1370 ikarashiMDC_2byte_ver omni_wheel SerialMultiByte Servo

main.cpp@5:885bffdceaa2, 23 months ago (annotated)

Committer:
me33004m
Date:
Sun Oct 09 06:34:58 2022 +0000
Revision:
5:885bffdceaa2
Parent:
4:82bc689e183e
Child:
6:d4b82ba4836a
page 172 change code

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