Ryu Anzawa / DoxygenTest2

doxygen test

main.cpp@2:98b9c6e44ae4, 2022-06-19 (annotated)

Committer:
piroro4560
Date:
Sun Jun 19 07:34:09 2022 +0000
Revision:
2:98b9c6e44ae4
Parent:
1:e6a7e0b92032
Child:
3:d3e52f43b427
test3

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