Ryu Anzawa / DoxygenTest2

doxygen test

main.cpp@0:6a6e78ee4e08, 2022-06-19 (annotated)

Committer:
piroro4560
Date:
Sun Jun 19 06:45:51 2022 +0000
Revision:
0:6a6e78ee4e08
Child:
1:e6a7e0b92032
doxygen test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 0:6a6e78ee4e08 1 /// @file main.cpp
piroro4560 0:6a6e78ee4e08 2 /// @brief ロボカップラジコンのプログラム
piroro4560 0:6a6e78ee4e08 3 ///
piroro4560 0:6a6e78ee4e08 4 /// 右端トグルは速度調整、中央２つでモード切り替えを行う
piroro4560 0:6a6e78ee4e08 5 ///
piroro4560 0:6a6e78ee4e08 6 ///
piroro4560 0:6a6e78ee4e08 7 ///
piroro4560 0:6a6e78ee4e08 8
piroro4560 0:6a6e78ee4e08 9 //--------操作方法--------
piroro4560 0:6a6e78ee4e08 10
piroro4560 0:6a6e78ee4e08 11
piroro4560 0:6a6e78ee4e08 12 #include "Master.h"
piroro4560 0:6a6e78ee4e08 13 #include "jy901.h"
piroro4560 0:6a6e78ee4e08 14 #include "PID.h"
piroro4560 0:6a6e78ee4e08 15 #include "SerialArduino.h"
piroro4560 0:6a6e78ee4e08 16
piroro4560 0:6a6e78ee4e08 17 DigitalOut led(LED1);
piroro4560 0:6a6e78ee4e08 18 Master Master;// 出力等の基本クラス
piroro4560 0:6a6e78ee4e08 19 OmniWheel omni(4);
piroro4560 0:6a6e78ee4e08 20 SerialArduino mini(A0, A1, 115200);// Arduinoを用いてPS3コントローラと通信するクラス
piroro4560 0:6a6e78ee4e08 21 JY901 jy(jysda, jyscl);
piroro4560 0:6a6e78ee4e08 22 PID Rotate(6.0, 50.0, 0.0005, 0.01);
piroro4560 0:6a6e78ee4e08 23 Timer Time_Shot;
piroro4560 0:6a6e78ee4e08 24
piroro4560 0:6a6e78ee4e08 25 void PS3get(bool flag_PS3print);
piroro4560 0:6a6e78ee4e08 26
piroro4560 0:6a6e78ee4e08 27 uint8_t h1,h2;
piroro4560 0:6a6e78ee4e08 28 bool b[12]= {};
piroro4560 0:6a6e78ee4e08 29 uint8_t st[4]= {};
piroro4560 0:6a6e78ee4e08 30 uint8_t tr[2]= {};
piroro4560 0:6a6e78ee4e08 31
piroro4560 0:6a6e78ee4e08 32 int main()
piroro4560 0:6a6e78ee4e08 33 {
piroro4560 0:6a6e78ee4e08 34 uint8_t mode=0;
piroro4560 0:6a6e78ee4e08 35 bool EdgeToggle[3]={}, pretoggle[3]={};
piroro4560 0:6a6e78ee4e08 36 bool flag_Dribble=false, flag_Shot=false, flag_Rotate;
piroro4560 0:6a6e78ee4e08 37 Time_Shot.start();
piroro4560 0:6a6e78ee4e08 38
piroro4560 0:6a6e78ee4e08 39 double pwm[4]={}, DribblePower = 0.5;
piroro4560 0:6a6e78ee4e08 40 double Vx=0, Vy=0, Vr=0, Rotate_Power=0, V=0, theta=0;
piroro4560 0:6a6e78ee4e08 41 double rawAngle, angleLimit;
piroro4560 0:6a6e78ee4e08 42
piroro4560 0:6a6e78ee4e08 43 Master.SetPS3Address(b, tr, st);
piroro4560 0:6a6e78ee4e08 44 jy.calibrateAll(50);
piroro4560 0:6a6e78ee4e08 45
piroro4560 0:6a6e78ee4e08 46 // DisplayThread.start(Display);
piroro4560 0:6a6e78ee4e08 47 #ifdef RobotNumberIsOne
piroro4560 0:6a6e78ee4e08 48 // 機体ナンバーが　１
piroro4560 0:6a6e78ee4e08 49 double VxLimit=0.75; // Ｘ軸方向ベクトルの最大値
piroro4560 0:6a6e78ee4e08 50 double VyLimit=0.75; // Ｙ軸方向ベクトルの最大値
piroro4560 0:6a6e78ee4e08 51 double VrLimit=0.4; // 回転方向ベクトルの最大値
piroro4560 0:6a6e78ee4e08 52 double Bias_Motor=0.75; // モーター出力値のバイアス
piroro4560 0:6a6e78ee4e08 53 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
piroro4560 0:6a6e78ee4e08 54 omni.wheel[1].setRadian(PI * 7.0 / 4.0);
piroro4560 0:6a6e78ee4e08 55 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
piroro4560 0:6a6e78ee4e08 56 omni.wheel[3].setRadian(PI * 3.0 / 4.0);
piroro4560 0:6a6e78ee4e08 57 #else
piroro4560 0:6a6e78ee4e08 58 // 機体ナンバーが　２
piroro4560 0:6a6e78ee4e08 59 double VxLimit=0.8; // Ｘ軸方向ベクトルの最大値
piroro4560 0:6a6e78ee4e08 60 double VyLimit=0.8; // Ｙ軸方向ベクトルの最大値
piroro4560 0:6a6e78ee4e08 61 double VrLimit=0.6; // 回転方向ベクトルの最大値
piroro4560 0:6a6e78ee4e08 62 double Bias_Motor=0.8; // モーター出力値のバイアス
piroro4560 0:6a6e78ee4e08 63 double Bias_DefectiveMD = 0.6; // ダメダメＭＤ補正用バイアス
piroro4560 0:6a6e78ee4e08 64 omni.wheel[0].setRadian(PI * 5.0 / 4.0);
piroro4560 0:6a6e78ee4e08 65 omni.wheel[1].setRadian(PI * 7.0 / 4.0);
piroro4560 0:6a6e78ee4e08 66 omni.wheel[2].setRadian(PI * 1.0 / 4.0);
piroro4560 0:6a6e78ee4e08 67 omni.wheel[3].setRadian(PI * 3.0 / 4.0);
piroro4560 0:6a6e78ee4e08 68 #endif
piroro4560 0:6a6e78ee4e08 69
piroro4560 0:6a6e78ee4e08 70 Rotate.setInputLimits(-180, 180);
piroro4560 0:6a6e78ee4e08 71 Rotate.setOutputLimits(-1*VrLimit,VrLimit);
piroro4560 0:6a6e78ee4e08 72 Rotate.setBias(0);
piroro4560 0:6a6e78ee4e08 73 Rotate.setMode(1);
piroro4560 0:6a6e78ee4e08 74 Rotate.setSetPoint(0);
piroro4560 0:6a6e78ee4e08 75
piroro4560 0:6a6e78ee4e08 76 // 以下ループ
piroro4560 0:6a6e78ee4e08 77 while (true) {
piroro4560 0:6a6e78ee4e08 78 if (mini.getState()) {
piroro4560 0:6a6e78ee4e08 79 rawAngle = jy.getZaxisAngle();
piroro4560 0:6a6e78ee4e08 80 if(rawAngle > 180.0) rawAngle -= 360;
piroro4560 0:6a6e78ee4e08 81
piroro4560 0:6a6e78ee4e08 82 angleLimit = rawAngle;
piroro4560 0:6a6e78ee4e08 83 Rotate.setProcessValue(angleLimit/*-sensor_.ballAngle*/);
piroro4560 0:6a6e78ee4e08 84 Rotate_Power = Rotate.compute();
piroro4560 0:6a6e78ee4e08 85
piroro4560 0:6a6e78ee4e08 86 PS3get(true);
piroro4560 0:6a6e78ee4e08 87
piroro4560 0:6a6e78ee4e08 88 // printf("angle : %d|power : %d | ", (int)rawAngle, (int)(Rotate_Power*100));
piroro4560 0:6a6e78ee4e08 89 printf("(Vx,Vy):(%3d,%3d)|theta:%d|angle:%d | ", (int)(Vx*100), (int)(Vy*100), (int)(theta), (int)angleLimit);
piroro4560 0:6a6e78ee4e08 90
piroro4560 0:6a6e78ee4e08 91 if (fabs(st[2]-128.0) > 50) {
piroro4560 0:6a6e78ee4e08 92 Rotate.setSetPoint(angleLimit);
piroro4560 0:6a6e78ee4e08 93 flag_Rotate = true;
piroro4560 0:6a6e78ee4e08 94 } else {
piroro4560 0:6a6e78ee4e08 95 flag_Rotate = false;
piroro4560 0:6a6e78ee4e08 96 }
piroro4560 0:6a6e78ee4e08 97
piroro4560 0:6a6e78ee4e08 98 if (b[1]) {
piroro4560 0:6a6e78ee4e08 99 jy.yawcalibrate();
piroro4560 0:6a6e78ee4e08 100 Rotate.setSetPoint(0);
piroro4560 0:6a6e78ee4e08 101 }
piroro4560 0:6a6e78ee4e08 102
piroro4560 0:6a6e78ee4e08 103 // モーター出力値の計算
piroro4560 0:6a6e78ee4e08 104 #ifdef RobotNumberIsOne
piroro4560 0:6a6e78ee4e08 105 // 機体ナンバーが　１
piroro4560 0:6a6e78ee4e08 106 Vx = ((st[0]-128.0)/128.0) * VxLimit;
piroro4560 0:6a6e78ee4e08 107 Vy = (128.0-st[1])/128.0 * VyLimit;
piroro4560 0:6a6e78ee4e08 108 Vr = -(st[2]-128.0)/128.0 * VrLimit * 0.5;
piroro4560 0:6a6e78ee4e08 109 #else
piroro4560 0:6a6e78ee4e08 110 // 機体ナンバーが　２
piroro4560 0:6a6e78ee4e08 111 Vx = ((st[0]-128.0)/128.0) * VxLimit;
piroro4560 0:6a6e78ee4e08 112 Vy = (128.0-st[1])/128.0 * VyLimit;
piroro4560 0:6a6e78ee4e08 113 Vr = -(st[2]-128.0)/128.0 * VrLimit * 0.5;
piroro4560 0:6a6e78ee4e08 114 #endif
piroro4560 0:6a6e78ee4e08 115 if (!flag_Rotate) {
piroro4560 0:6a6e78ee4e08 116 Vr = Rotate_Power;
piroro4560 0:6a6e78ee4e08 117 }
piroro4560 0:6a6e78ee4e08 118
piroro4560 0:6a6e78ee4e08 119 V = hypot(Vx, Vy);
piroro4560 0:6a6e78ee4e08 120 theta = atan2(Vy, Vx);
piroro4560 0:6a6e78ee4e08 121 #ifdef RobotNumberIsOne
piroro4560 0:6a6e78ee4e08 122 omni.computeCircular(V, theta - angleLimit/180.0*PI, Vr);
piroro4560 0:6a6e78ee4e08 123 #else
piroro4560 0:6a6e78ee4e08 124 omni.computeCircular(V, theta - angleLimit/180.0*PI, -Vr);
piroro4560 0:6a6e78ee4e08 125 #endif
piroro4560 0:6a6e78ee4e08 126
piroro4560 0:6a6e78ee4e08 127 for(int i=0; i < 4; i++) {
piroro4560 0:6a6e78ee4e08 128 if (0) {
piroro4560 0:6a6e78ee4e08 129 if (b[i+4]) {
piroro4560 0:6a6e78ee4e08 130 pwm[i] = 0.5;
piroro4560 0:6a6e78ee4e08 131 // printf("%d ", i);
piroro4560 0:6a6e78ee4e08 132 } else if (b[i+4+4]) {
piroro4560 0:6a6e78ee4e08 133 pwm[i] = -0.5;
piroro4560 0:6a6e78ee4e08 134 } else {
piroro4560 0:6a6e78ee4e08 135 pwm[i] = 0;
piroro4560 0:6a6e78ee4e08 136 }
piroro4560 0:6a6e78ee4e08 137 } else {
piroro4560 0:6a6e78ee4e08 138 pwm[i] = ((double)omni.wheel[i] * Bias_Motor);
piroro4560 0:6a6e78ee4e08 139 }
piroro4560 0:6a6e78ee4e08 140 }
piroro4560 0:6a6e78ee4e08 141 // printf("\r\n");
piroro4560 0:6a6e78ee4e08 142
piroro4560 0:6a6e78ee4e08 143 // フラグ処理
piroro4560 0:6a6e78ee4e08 144 if (b[9]) flag_Dribble = true;
piroro4560 0:6a6e78ee4e08 145 else flag_Dribble = false;
piroro4560 0:6a6e78ee4e08 146
piroro4560 0:6a6e78ee4e08 147 if (b[8] && (Time_Shot.read_ms() > 1000)) {
piroro4560 0:6a6e78ee4e08 148 flag_Shot = true;
piroro4560 0:6a6e78ee4e08 149 Time_Shot.reset();
piroro4560 0:6a6e78ee4e08 150 } else {
piroro4560 0:6a6e78ee4e08 151 flag_Shot = false;
piroro4560 0:6a6e78ee4e08 152 }
piroro4560 0:6a6e78ee4e08 153
piroro4560 0:6a6e78ee4e08 154 // ドリブラー・キッカー
piroro4560 0:6a6e78ee4e08 155 // if (flag_Dribble) esc.setspeed(0.7);
piroro4560 0:6a6e78ee4e08 156 // else esc.setspeed(0.0);
piroro4560 0:6a6e78ee4e08 157 if (flag_Dribble) Master.Dribble(0.7);
piroro4560 0:6a6e78ee4e08 158 else Master.Dribble(0.0);
piroro4560 0:6a6e78ee4e08 159 if (flag_Shot) Master.Shot();
piroro4560 0:6a6e78ee4e08 160
piroro4560 0:6a6e78ee4e08 161 // ニュートラル処理
piroro4560 0:6a6e78ee4e08 162 for (int i=0; i<4; i++) {
piroro4560 0:6a6e78ee4e08 163 if (fabs(pwm[i]) < 0.1) pwm[i] = 0.0;
piroro4560 0:6a6e78ee4e08 164 }
piroro4560 0:6a6e78ee4e08 165
piroro4560 0:6a6e78ee4e08 166 #ifdef RobotNumberIsOne
piroro4560 0:6a6e78ee4e08 167 Master.SetValueMotor(0, pwm[0]);
piroro4560 0:6a6e78ee4e08 168 Master.SetValueMotor(1, pwm[1]);
piroro4560 0:6a6e78ee4e08 169 Master.SetValueMotor(2, -pwm[2]);
piroro4560 0:6a6e78ee4e08 170 Master.SetValueMotor(3, -pwm[3]);
piroro4560 0:6a6e78ee4e08 171 #else
piroro4560 0:6a6e78ee4e08 172 Master.SetValueMotor(0, pwm[0] * Bias_DefectiveMD);
piroro4560 0:6a6e78ee4e08 173 Master.SetValueMotor(1, pwm[1]);
piroro4560 0:6a6e78ee4e08 174 Master.SetValueMotor(2, pwm[2]);
piroro4560 0:6a6e78ee4e08 175 Master.SetValueMotor(3, pwm[3] * Bias_DefectiveMD);
piroro4560 0:6a6e78ee4e08 176 #endif
piroro4560 0:6a6e78ee4e08 177 }
piroro4560 0:6a6e78ee4e08 178 }
piroro4560 0:6a6e78ee4e08 179 }
piroro4560 0:6a6e78ee4e08 180
piroro4560 0:6a6e78ee4e08 181 void PS3get(bool flag_PS3print)
piroro4560 0:6a6e78ee4e08 182 {
piroro4560 0:6a6e78ee4e08 183 h1 = mini.getHedder1();
piroro4560 0:6a6e78ee4e08 184 h2 = mini.getHedder2();
piroro4560 0:6a6e78ee4e08 185 for(int i=0; i<12; i++) {
piroro4560 0:6a6e78ee4e08 186 b[i] = mini.getButton(i);
piroro4560 0:6a6e78ee4e08 187 }
piroro4560 0:6a6e78ee4e08 188 for(int i=0; i<2; i++) {
piroro4560 0:6a6e78ee4e08 189 tr[i] = mini.getTrigger(i);
piroro4560 0:6a6e78ee4e08 190 }
piroro4560 0:6a6e78ee4e08 191 for(int i=0; i<4; i++) {
piroro4560 0:6a6e78ee4e08 192 st[i] = mini.getStick(i);
piroro4560 0:6a6e78ee4e08 193 }
piroro4560 0:6a6e78ee4e08 194 if (flag_PS3print) {
piroro4560 0:6a6e78ee4e08 195 for(int i=0; i<12; i++) printf("%d ",b[i]);
piroro4560 0:6a6e78ee4e08 196 printf("|");
piroro4560 0:6a6e78ee4e08 197 for(int i=0; i<2; i++) printf("%3d ",tr[i]);
piroro4560 0:6a6e78ee4e08 198 printf("|");
piroro4560 0:6a6e78ee4e08 199 for(int i=0; i<4; i++) printf("%3d ",st[i]);
piroro4560 0:6a6e78ee4e08 200 if(mini.getState()) printf("ok");
piroro4560 0:6a6e78ee4e08 201 else printf("bad");
piroro4560 0:6a6e78ee4e08 202 printf("\r\n");
piroro4560 0:6a6e78ee4e08 203 } else {
piroro4560 0:6a6e78ee4e08 204 mini.getState();
piroro4560 0:6a6e78ee4e08 205 }
piroro4560 0:6a6e78ee4e08 206 }