abe takumi / Mbed OS 2022NHK_A_main

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

Dependencies:   FEP_RX22 OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver omni_wheel

main.cpp@2:d84346eaa720, 23 months ago (annotated)

Committer:
me33004m
Date:
Sat Oct 08 14:33:24 2022 +0000
Revision:
2:d84346eaa720
Parent:
1:5132f966db08
Child:
3:4c0c8046c3a7
That spaghetti code is only 7 lines! !; Thank you seniors for your guidance.; After that, I put the library of FEP_RX22 and made it a new controller.; I don't know if it will work.; plus I'm not sure if this text translated into English properly.

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 1:5132f966db08 11
me33004m 2:d84346eaa720 12 FEP_RX22 mycon(fepTX, fepRX, fepad);
me33004m 0:5d4705b2893c 13 Serial pc(pcTX, pcRX, 115200);
me33004m 0:5d4705b2893c 14 Serial RS485(PC_10,PC_11,115200);
me33004m 1:5132f966db08 15
me33004m 1:5132f966db08 16
me33004m 0:5d4705b2893c 17 uint8_t b[8];
me33004m 0:5d4705b2893c 18 int16_t stick[4];
me33004m 2:d84346eaa720 19 int16_t trigger[4];
me33004m 0:5d4705b2893c 20 double engine[4];
me33004m 0:5d4705b2893c 21 double speed[6];
me33004m 0:5d4705b2893c 22 double spin_power;
me33004m 0:5d4705b2893c 23 double posiX , posiY , posiTheta;
me33004m 1:5132f966db08 24
me33004m 0:5d4705b2893c 25 DigitalOut stop(A3);
me33004m 0:5d4705b2893c 26 ikarashiMDC motor[] = {
me33004m 0:5d4705b2893c 27 ikarashiMDC(0,0,SM,&RS485),
me33004m 0:5d4705b2893c 28 ikarashiMDC(0,1,SM,&RS485),
me33004m 0:5d4705b2893c 29 ikarashiMDC(0,2,SM,&RS485),
me33004m 0:5d4705b2893c 30 ikarashiMDC(0,3,SM,&RS485),
me33004m 0:5d4705b2893c 31 };
me33004m 1:5132f966db08 32
me33004m 0:5d4705b2893c 33 OmniWheel omni(4);
me33004m 0:5d4705b2893c 34 OmniPosition posi(mwTX, mwRX);
me33004m 1:5132f966db08 35
me33004m 0:5d4705b2893c 36 PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！
me33004m 1:5132f966db08 37
me33004m 0:5d4705b2893c 38 /*プロトタイプ宣言*/
me33004m 0:5d4705b2893c 39 void chassis();//足回りの制御・ジャイロ・テラターム
me33004m 0:5d4705b2893c 40 void spin(double ang);//PID
me33004m 2:d84346eaa720 41 int pm(double num); //正負判定
me33004m 1:5132f966db08 42
me33004m 0:5d4705b2893c 43 int main(void){
me33004m 0:5d4705b2893c 44 mycon.StartReceive();
me33004m 1:5132f966db08 45
me33004m 0:5d4705b2893c 46 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
me33004m 0:5d4705b2893c 47 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
me33004m 0:5d4705b2893c 48 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
me33004m 0:5d4705b2893c 49 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
me33004m 0:5d4705b2893c 50
me33004m 0:5d4705b2893c 51 angle.setInputLimits(-180,180);
me33004m 0:5d4705b2893c 52 angle.setOutputLimits(-0.4,0.4);
me33004m 0:5d4705b2893c 53 angle.setBias(0);
me33004m 0:5d4705b2893c 54 angle.setMode(1);
me33004m 0:5d4705b2893c 55 angle.setSetPoint(0);
me33004m 0:5d4705b2893c 56 while(1){
me33004m 0:5d4705b2893c 57 stop = 1;
me33004m 0:5d4705b2893c 58 chassis();
me33004m 0:5d4705b2893c 59 }
me33004m 0:5d4705b2893c 60
me33004m 0:5d4705b2893c 61 }
me33004m 1:5132f966db08 62
me33004m 0:5d4705b2893c 63 void chassis(){
me33004m 0:5d4705b2893c 64 /*非常停止*/
me33004m 0:5d4705b2893c 65 //stop = 1;
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 0:5d4705b2893c 74 for (int i=0; i<8; 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 2:d84346eaa720 94 engine[i] = 0.4*(stick[i]/128.0)*(trigger[1]/256.0);
me33004m 0:5d4705b2893c 95 }else{
me33004m 0:5d4705b2893c 96 engine[i] = 0;
me33004m 0:5d4705b2893c 97 }
me33004m 0:5d4705b2893c 98 }
me33004m 0:5d4705b2893c 99 /*PID*/
me33004m 0:5d4705b2893c 100 /*スパゲッティコードで申し訳ないです...*/
me33004m 2:d84346eaa720 101 if(abs(stick[2]) < 10){/*
me33004m 0:5d4705b2893c 102 if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){
me33004m 0:5d4705b2893c 103 spin(0);
me33004m 1:5132f966db08 104 pc.printf("a");
me33004m 0:5d4705b2893c 105 }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){
me33004m 0:5d4705b2893c 106 spin(15);
me33004m 1:5132f966db08 107 pc.printf("b");
me33004m 0:5d4705b2893c 108 }else if(((posiTheta>=22.5)&&(posiTheta<37.5))&&((posiTheta>31)||(posiTheta<29))){
me33004m 0:5d4705b2893c 109 spin(30);
me33004m 0:5d4705b2893c 110 }else if(((posiTheta>=37.5)&&(posiTheta<52.5))&&((posiTheta>46)||(posiTheta<44))){
me33004m 0:5d4705b2893c 111 spin(45);
me33004m 0:5d4705b2893c 112 }else if(((posiTheta>=52.5)&&(posiTheta<67.5))&&((posiTheta>61)||(posiTheta<59))){
me33004m 0:5d4705b2893c 113 spin(60);
me33004m 0:5d4705b2893c 114 }else if(((posiTheta>=67.5)&&(posiTheta<82.5))&&((posiTheta>76)||(posiTheta<74))){
me33004m 0:5d4705b2893c 115 spin(75);
me33004m 0:5d4705b2893c 116 }else if(((posiTheta>=82.5)&&(posiTheta<97.5))&&((posiTheta>91)||(posiTheta<89))){
me33004m 0:5d4705b2893c 117 spin(90);
me33004m 0:5d4705b2893c 118 }else if(((posiTheta>=97.5)&&(posiTheta<112.5))&&((posiTheta>106)||(posiTheta<104))){
me33004m 0:5d4705b2893c 119 spin(105);
me33004m 0:5d4705b2893c 120 }else if(((posiTheta>=112.5)&&(posiTheta<127.5))&&((posiTheta>121)||(posiTheta<119))){
me33004m 0:5d4705b2893c 121 spin(120);
me33004m 0:5d4705b2893c 122 }else if(((posiTheta>=127.5)&&(posiTheta<142.5))&&((posiTheta>136)||(posiTheta<134))){
me33004m 0:5d4705b2893c 123 spin(135);
me33004m 0:5d4705b2893c 124 }else if(((posiTheta>=142.5)&&(posiTheta<157.5))&&((posiTheta>151)||(posiTheta<149))){
me33004m 0:5d4705b2893c 125 spin(150);
me33004m 0:5d4705b2893c 126 }else if(((posiTheta>=157.5)&&(posiTheta<172.5))&&((posiTheta>166)||(posiTheta<164))){
me33004m 0:5d4705b2893c 127 spin(165);
me33004m 0:5d4705b2893c 128 }else if(((posiTheta>=172.5)&&(posiTheta<=179))||((posiTheta<=-172.5)&&(posiTheta>=-179))){
me33004m 0:5d4705b2893c 129 spin(180);
me33004m 0:5d4705b2893c 130 }else if(((posiTheta<=-157.5)&&(posiTheta>-172.5))&&((posiTheta>-164)||(posiTheta<-166))){
me33004m 0:5d4705b2893c 131 spin(-165);
me33004m 0:5d4705b2893c 132 }else if(((posiTheta<=-142.5)&&(posiTheta>-157.5))&&((posiTheta>-149)||(posiTheta<-151))){
me33004m 0:5d4705b2893c 133 spin(-150);
me33004m 0:5d4705b2893c 134 }else if(((posiTheta<=-127.5)&&(posiTheta>-142.5))&&((posiTheta>-134)||(posiTheta<-136))){
me33004m 0:5d4705b2893c 135 spin(-135);
me33004m 0:5d4705b2893c 136 }else if(((posiTheta<=-112.5)&&(posiTheta>-127.5))&&((posiTheta>-119)||(posiTheta<-121))){
me33004m 0:5d4705b2893c 137 spin(-120);
me33004m 0:5d4705b2893c 138 }else if(((posiTheta<=-97.5)&&(posiTheta>-112.5))&&((posiTheta>-104)||(posiTheta<-106))){
me33004m 0:5d4705b2893c 139 spin(-105);
me33004m 0:5d4705b2893c 140 }else if(((posiTheta<=-82.5)&&(posiTheta>-90.5))&&((posiTheta>-89)||(posiTheta<-91))){
me33004m 0:5d4705b2893c 141 spin(-90);
me33004m 0:5d4705b2893c 142 }else if(((posiTheta<=-67.5)&&(posiTheta>-82.5))&&((posiTheta>-74)||(posiTheta<-76))){
me33004m 0:5d4705b2893c 143 spin(-75);
me33004m 0:5d4705b2893c 144 }else if(((posiTheta<=-52.5)&&(posiTheta>-67.5))&&((posiTheta>-59)||(posiTheta<-61))){
me33004m 0:5d4705b2893c 145 spin(-60);
me33004m 0:5d4705b2893c 146 }else if(((posiTheta<=-37.5)&&(posiTheta>-52.5))&&((posiTheta>-44)||(posiTheta<-46))){
me33004m 0:5d4705b2893c 147 spin(-45);
me33004m 0:5d4705b2893c 148 }else if(((posiTheta<=-22.5)&&(posiTheta>-37.5))&&((posiTheta>-29)||(posiTheta<-30))){
me33004m 0:5d4705b2893c 149 spin(-30);
me33004m 0:5d4705b2893c 150 }else if(((posiTheta<=-7.5)&&(posiTheta>-22.5))&&((posiTheta>-14)||(posiTheta<-16))){
me33004m 0:5d4705b2893c 151 spin(-15);
me33004m 0:5d4705b2893c 152 }else{
me33004m 2:d84346eaa720 153 }*/
me33004m 2:d84346eaa720 154 /*for(int i=-180;i<=180;i+=15){
me33004m 2:d84346eaa720 155 if(((posiTheta>=i-7.5)&&(posiTheta<i+7.5))&&((posiTheta>i+1)||(posiTheta<i-1))){
me33004m 2:d84346eaa720 156 spin(i);
me33004m 2:d84346eaa720 157 }
me33004m 2:d84346eaa720 158 else if((posiTheta == i) ||(posiTheta == i+1) ||(posiTheta == i-1)){
me33004m 2:d84346eaa720 159 }
me33004m 2:d84346eaa720 160 }*/
me33004m 2:d84346eaa720 161 if(fabs(TargetAngle-posiTheta)>8){
me33004m 2:d84346eaa720 162 TargetAngle += 15*pm(posiTheta-TargetAngle);
me33004m 2:d84346eaa720 163 spin(TargetAngle);
me33004m 2:d84346eaa720 164 if(abs(TargetAngle)==180){
me33004m 2:d84346eaa720 165 TargetAngle += -360*pm(TargetAngle);
me33004m 2:d84346eaa720 166 }
me33004m 2:d84346eaa720 167 }
me33004m 0:5d4705b2893c 168 }
me33004m 1:5132f966db08 169
me33004m 1:5132f966db08 170 /*旋回*/
me33004m 1:5132f966db08 171 if(abs(stick[2]) > 10){
me33004m 1:5132f966db08 172 spin_power = engine[2];
me33004m 1:5132f966db08 173 }else{
me33004m 1:5132f966db08 174 spin_power = angle.compute();
me33004m 1:5132f966db08 175 }
me33004m 1:5132f966db08 176
me33004m 1:5132f966db08 177 omni.computeXY(engine[0],engine[1],-spin_power);
me33004m 1:5132f966db08 178 for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
me33004m 1:5132f966db08 179
me33004m 1:5132f966db08 180
me33004m 1:5132f966db08 181 for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
me33004m 1:5132f966db08 182
me33004m 0:5d4705b2893c 183 }
me33004m 0:5d4705b2893c 184 void spin(double ang)
me33004m 0:5d4705b2893c 185 {
me33004m 0:5d4705b2893c 186 angle.setSetPoint(ang);
me33004m 2:d84346eaa720 187 posiTheta = posi.getTheta()*(180.0/M_PI);
me33004m 2:d84346eaa720 188 angle.setProcessValue(posiTheta);
me33004m 2:d84346eaa720 189 pc.printf("/r/nang:%.2f pid:%.2f posi:%.2f\r\n",ang,-angle.compute(),posiTheta);
me33004m 2:d84346eaa720 190 //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
me33004m 2:d84346eaa720 191 }
me33004m 2:d84346eaa720 192 int pm(double num){
me33004m 2:d84346eaa720 193 return((num>0)-(num<0));
me33004m 0:5d4705b2893c 194 }