NagaokaRoboticsClub_mbedTeam / Mbed OS 2022NHK_A_main

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

Dependencies:   FEP_RX22 OmniPosition PID R1370 ikarashiMDC_2byte_ver omni_wheel SerialMultiByte Servo

main.cpp@0:5d4705b2893c, 23 months ago (annotated)

Committer:
me33004m
Date:
Fri Oct 07 10:25:24 2022 +0000
Revision:
0:5d4705b2893c
Child:
1:5132f966db08
ashimawari

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 "controller.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 0:5d4705b2893c 9
me33004m 0:5d4705b2893c 10 #define PI 3.141592653589
me33004m 0:5d4705b2893c 11
me33004m 0:5d4705b2893c 12 Bcon 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 0:5d4705b2893c 15
me33004m 0:5d4705b2893c 16
me33004m 0:5d4705b2893c 17 uint8_t b[8];
me33004m 0:5d4705b2893c 18 int16_t stick[4];
me33004m 0:5d4705b2893c 19 double engine[4];
me33004m 0:5d4705b2893c 20 double speed[6];
me33004m 0:5d4705b2893c 21 double spin_power;
me33004m 0:5d4705b2893c 22 double posiX , posiY , posiTheta;
me33004m 0:5d4705b2893c 23
me33004m 0:5d4705b2893c 24 DigitalOut stop(A3);
me33004m 0:5d4705b2893c 25 ikarashiMDC motor[] = {
me33004m 0:5d4705b2893c 26 ikarashiMDC(0,0,SM,&RS485),
me33004m 0:5d4705b2893c 27 ikarashiMDC(0,1,SM,&RS485),
me33004m 0:5d4705b2893c 28 ikarashiMDC(0,2,SM,&RS485),
me33004m 0:5d4705b2893c 29 ikarashiMDC(0,3,SM,&RS485),
me33004m 0:5d4705b2893c 30 };
me33004m 0:5d4705b2893c 31
me33004m 0:5d4705b2893c 32 OmniWheel omni(4);
me33004m 0:5d4705b2893c 33 OmniPosition posi(mwTX, mwRX);
me33004m 0:5d4705b2893c 34
me33004m 0:5d4705b2893c 35 PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう！
me33004m 0:5d4705b2893c 36
me33004m 0:5d4705b2893c 37 /*プロトタイプ宣言*/
me33004m 0:5d4705b2893c 38 void chassis();//足回りの制御・ジャイロ・テラターム
me33004m 0:5d4705b2893c 39 void spin(double ang);//PID
me33004m 0:5d4705b2893c 40
me33004m 0:5d4705b2893c 41 int main(void){
me33004m 0:5d4705b2893c 42 mycon.StartReceive();
me33004m 0:5d4705b2893c 43
me33004m 0:5d4705b2893c 44 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
me33004m 0:5d4705b2893c 45 omni.wheel[1].setRadian(PI * 3.0 / 4.0);
me33004m 0:5d4705b2893c 46 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
me33004m 0:5d4705b2893c 47 omni.wheel[3].setRadian(PI * 7.0 / 4.0);
me33004m 0:5d4705b2893c 48
me33004m 0:5d4705b2893c 49 angle.setInputLimits(-180,180);
me33004m 0:5d4705b2893c 50 angle.setOutputLimits(-0.4,0.4);
me33004m 0:5d4705b2893c 51 angle.setBias(0);
me33004m 0:5d4705b2893c 52 angle.setMode(1);
me33004m 0:5d4705b2893c 53 angle.setSetPoint(0);
me33004m 0:5d4705b2893c 54 while(1){
me33004m 0:5d4705b2893c 55 stop = 1;
me33004m 0:5d4705b2893c 56 chassis();
me33004m 0:5d4705b2893c 57 }
me33004m 0:5d4705b2893c 58
me33004m 0:5d4705b2893c 59 }
me33004m 0:5d4705b2893c 60
me33004m 0:5d4705b2893c 61 void chassis(){
me33004m 0:5d4705b2893c 62 /*非常停止*/
me33004m 0:5d4705b2893c 63 //stop = 1;
me33004m 0:5d4705b2893c 64
me33004m 0:5d4705b2893c 65 /*ジャイロ読み取り*/
me33004m 0:5d4705b2893c 66 posiX = posi.getX();
me33004m 0:5d4705b2893c 67 posiY = posi.getY();
me33004m 0:5d4705b2893c 68 posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換
me33004m 0:5d4705b2893c 69 pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta);
me33004m 0:5d4705b2893c 70 /*コントローラー受信*/
me33004m 0:5d4705b2893c 71 for (int i=0; i<8; i++) b[i] = mycon.getButton(i);
me33004m 0:5d4705b2893c 72 for (int i=0; i<4; i++) stick[i] = mycon.getStick(i);
me33004m 0:5d4705b2893c 73
me33004m 0:5d4705b2893c 74 for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
me33004m 0:5d4705b2893c 75 pc.printf(" | ");
me33004m 0:5d4705b2893c 76 for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
me33004m 0:5d4705b2893c 77 pc.printf(" | ");
me33004m 0:5d4705b2893c 78 if (mycon.status) pc.printf("received\r\n");
me33004m 0:5d4705b2893c 79 else pc.printf("anything error...\r\n");
me33004m 0:5d4705b2893c 80
me33004m 0:5d4705b2893c 81 for(int i=0; i<4; i++){
me33004m 0:5d4705b2893c 82 speed[i] = 0;
me33004m 0:5d4705b2893c 83 }
me33004m 0:5d4705b2893c 84
me33004m 0:5d4705b2893c 85 /*notcontoroler*/
me33004m 0:5d4705b2893c 86 for(int i=0; i<4; i++){
me33004m 0:5d4705b2893c 87 motor[i].setSpeed(0);
me33004m 0:5d4705b2893c 88 }
me33004m 0:5d4705b2893c 89 /*全方位*/
me33004m 0:5d4705b2893c 90 for(int i=0; i<4; i++){
me33004m 0:5d4705b2893c 91 if(abs(stick[i]) > 10){
me33004m 0:5d4705b2893c 92 engine[i] = 0.4*(stick[i]/128.0);
me33004m 0:5d4705b2893c 93 }else{
me33004m 0:5d4705b2893c 94 engine[i] = 0;
me33004m 0:5d4705b2893c 95 }
me33004m 0:5d4705b2893c 96 }
me33004m 0:5d4705b2893c 97
me33004m 0:5d4705b2893c 98 /*旋回*/
me33004m 0:5d4705b2893c 99 if(abs(stick[2]) > 10){
me33004m 0:5d4705b2893c 100 spin_power = engine[2];
me33004m 0:5d4705b2893c 101 }else{
me33004m 0:5d4705b2893c 102 spin_power = 0;
me33004m 0:5d4705b2893c 103 }
me33004m 0:5d4705b2893c 104
me33004m 0:5d4705b2893c 105 omni.computeXY(engine[0],engine[1],-spin_power);
me33004m 0:5d4705b2893c 106 for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i];
me33004m 0:5d4705b2893c 107
me33004m 0:5d4705b2893c 108
me33004m 0:5d4705b2893c 109 for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]);
me33004m 0:5d4705b2893c 110 /*PID*/
me33004m 0:5d4705b2893c 111 /*スパゲッティコードで申し訳ないです...*/
me33004m 0:5d4705b2893c 112 if(abs(stick[2]) < 10){
me33004m 0:5d4705b2893c 113 if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){
me33004m 0:5d4705b2893c 114 spin(0);
me33004m 0:5d4705b2893c 115 }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){
me33004m 0:5d4705b2893c 116 spin(15);
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 0:5d4705b2893c 162 }
me33004m 0:5d4705b2893c 163 }
me33004m 0:5d4705b2893c 164 }
me33004m 0:5d4705b2893c 165 void spin(double ang)
me33004m 0:5d4705b2893c 166 {
me33004m 0:5d4705b2893c 167 angle.setSetPoint(ang);
me33004m 0:5d4705b2893c 168 while(1) {
me33004m 0:5d4705b2893c 169 stop = 1;
me33004m 0:5d4705b2893c 170 posiTheta = posi.getTheta()*(180.0/M_PI);
me33004m 0:5d4705b2893c 171 angle.setProcessValue(posiTheta);
me33004m 0:5d4705b2893c 172 pc.printf("spinning... |%d|%.2f|%.3f\r\n",ang,posiTheta,-angle.compute());
me33004m 0:5d4705b2893c 173 for(int i=0; i<4; i++) motor[i].setSpeed(-angle.compute());
me33004m 0:5d4705b2893c 174 //for(int i=4; i<8; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す
me33004m 0:5d4705b2893c 175 if ((fabs(ang - posiTheta) < 1)||(fabs(ang - posiTheta)> 7.5)) return;
me33004m 0:5d4705b2893c 176 }
me33004m 0:5d4705b2893c 177 }