abe takumi / Mbed OS 2022NHK_A_ashimawari

q

Dependencies:   FEP_RX22 OmniPosition PID R1370 ikarashiMDC_2byte_ver omni_wheel

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

Committer:
me33004m
Date:
Mon Oct 10 08:01:10 2022 +0000
Revision:
0:b7dbdc0b19f3
compureat;

Who changed what in which revision?

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