Ryu Anzawa / DoxygenTest2

doxygen test

main.cpp@1:e6a7e0b92032, 2022-06-19 (annotated)

Committer:
piroro4560
Date:
Sun Jun 19 07:25:28 2022 +0000
Revision:
1:e6a7e0b92032
Parent:
0:6a6e78ee4e08
Child:
2:98b9c6e44ae4
test2;

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