Ryu Anzawa / DoxygenTest2

doxygen test

main.cpp@5:62c9acd791ce, 2022-06-23 (annotated)

Committer:
piroro4560
Date:
Thu Jun 23 15:17:20 2022 +0000
Revision:
5:62c9acd791ce
Parent:
4:bbc6c27561e6
Child:
6:ac8204439a89
test5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 3:d3e52f43b427 1 ///
piroro4560 0:6a6e78ee4e08 2 /// @file main.cpp
piroro4560 0:6a6e78ee4e08 3 /// @brief ロボカップラジコンのプログラム
piroro4560 0:6a6e78ee4e08 4 ///
piroro4560 1:e6a7e0b92032 5 /// @note トグルは上が１で下が０
piroro4560 1:e6a7e0b92032 6 ///
piroro4560 1:e6a7e0b92032 7 /// @details 右端トグルで速度切り替え
piroro4560 1:e6a7e0b92032 8 /// @note １：速め　０：遅め
piroro4560 1:e6a7e0b92032 9 ///
piroro4560 1:e6a7e0b92032 10 /// @details 中央トグル２つでモード切替
piroro4560 3:d3e52f43b427 11 /// @note Refer
piroro4560 3:d3e52f43b427 12 /// - 中央トグルでのモード切替;
piroro4560 3:d3e52f43b427 13 /// - 左０右０：スタート
piroro4560 3:d3e52f43b427 14 /// - 左１右０：キッカーテスト
piroro4560 3:d3e52f43b427 15 /// - 左０右１：ドリブラーテスト
piroro4560 3:d3e52f43b427 16 /// - 左１右１：オムニテスト
piroro4560 3:d3e52f43b427 17 /// - ボタンを押すかトグルスイッチを変えるとワイヤレスモード中止
piroro4560 0:6a6e78ee4e08 18 ///
piroro4560 2:98b9c6e44ae4 19 /// @details スタート後にPSボタンを押してペアリング 以下操作方法
piroro4560 3:d3e52f43b427 20 /// @note Refer
piroro4560 3:d3e52f43b427 21 /// - 左スティックで移動(絶対角)
piroro4560 3:d3e52f43b427 22 /// - 右スティックX軸で回転
piroro4560 3:d3e52f43b427 23 /// - 〇でドリブル、△でシュート
piroro4560 0:6a6e78ee4e08 24 ///
piroro4560 1:e6a7e0b92032 25 /// @attention １番機は起動後にArduinoのリセットボタンを押す必要あり
piroro4560 1:e6a7e0b92032 26 ///
piroro4560 0:6a6e78ee4e08 27 #include "Master.h"
piroro4560 0:6a6e78ee4e08 28 #include "jy901.h"
piroro4560 0:6a6e78ee4e08 29 #include "PID.h"
piroro4560 3:d3e52f43b427 30 #include "omni_wheel.h"
piroro4560 0:6a6e78ee4e08 31 #include "SerialArduino.h"
piroro4560 0:6a6e78ee4e08 32
piroro4560 0:6a6e78ee4e08 33 DigitalOut led(LED1);
piroro4560 3:d3e52f43b427 34 DigitalIn toggle[3] = {
piroro4560 3:d3e52f43b427 35 DigitalIn(Pin_toggle_0),
piroro4560 3:d3e52f43b427 36 DigitalIn(Pin_toggle_1),
piroro4560 3:d3e52f43b427 37 DigitalIn(Pin_toggle_2)
piroro4560 3:d3e52f43b427 38 };
piroro4560 3:d3e52f43b427 39
piroro4560 3:d3e52f43b427 40 SerialArduino mini(Pin_Arduino_TX, Pin_Arduino_RX, 115200);
piroro4560 3:d3e52f43b427 41 Master Master;
piroro4560 3:d3e52f43b427 42 JY901 jy(Pin_JY901_sda, Pin_JY901_scl);
piroro4560 3:d3e52f43b427 43 DigitalIn rawButton(Pin_button);
piroro4560 0:6a6e78ee4e08 44 OmniWheel omni(4);
piroro4560 3:d3e52f43b427 45 Timer Time_Shot;
piroro4560 3:d3e52f43b427 46 Timer Time_MotorTest;
piroro4560 3:d3e52f43b427 47
piroro4560 1:e6a7e0b92032 48 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 49 PID Rotate(25.0, 100.0, 0.0001, 0.01);
piroro4560 1:e6a7e0b92032 50 #else
piroro4560 3:d3e52f43b427 51 PID Rotate(15.0, 100.0, 0.0001, 0.01);
piroro4560 1:e6a7e0b92032 52 #endif
piroro4560 3:d3e52f43b427 53
piroro4560 0:6a6e78ee4e08 54
piroro4560 3:d3e52f43b427 55 ///
piroro4560 3:d3e52f43b427 56 /// @fn PS3get(bool flag_PS3print)
piroro4560 3:d3e52f43b427 57 /// @brief ループ毎に実行。SerialArduinoライブラリを用いてPS3コントローラの値を取得。
piroro4560 3:d3e52f43b427 58 /// @param flag_PS3print 受け取った値を表示したい時にtrue
piroro4560 3:d3e52f43b427 59 ///
piroro4560 0:6a6e78ee4e08 60 void PS3get(bool flag_PS3print);
piroro4560 0:6a6e78ee4e08 61
piroro4560 3:d3e52f43b427 62 ///
piroro4560 3:d3e52f43b427 63 /// @fn StartDemo();
piroro4560 3:d3e52f43b427 64 /// DualShock3を用いたラジコン操作
piroro4560 3:d3e52f43b427 65 /// @brief modeが0の時にボタンを押すとスタート
piroro4560 3:d3e52f43b427 66 /// @attention ロボットの機体番号に応じて出力とピンを変える必要アリ
piroro4560 3:d3e52f43b427 67 ///
piroro4560 3:d3e52f43b427 68 void StartDemo();
piroro4560 3:d3e52f43b427 69
piroro4560 3:d3e52f43b427 70 ///
piroro4560 3:d3e52f43b427 71 /// @fn MotorTest()
piroro4560 3:d3e52f43b427 72 /// @brief モーターを4つ連続で回す。1秒で正転反転が切り替わる
piroro4560 3:d3e52f43b427 73 /// @brief modeが2の時にボタンを押すとスタート
piroro4560 3:d3e52f43b427 74 ///
piroro4560 3:d3e52f43b427 75 void MotorTest();
piroro4560 3:d3e52f43b427 76
piroro4560 3:d3e52f43b427 77 bool flag_Passfunc;
piroro4560 3:d3e52f43b427 78 uint8_t preButton;
piroro4560 3:d3e52f43b427 79 uint8_t Button;
piroro4560 3:d3e52f43b427 80 uint8_t Toggle_mode=0, Toggle_speed=0;
piroro4560 0:6a6e78ee4e08 81 uint8_t h1,h2;
piroro4560 3:d3e52f43b427 82 bool b[12]= {};
piroro4560 0:6a6e78ee4e08 83 uint8_t st[4]= {};
piroro4560 0:6a6e78ee4e08 84 uint8_t tr[2]= {};
piroro4560 0:6a6e78ee4e08 85
piroro4560 0:6a6e78ee4e08 86 int main()
piroro4560 0:6a6e78ee4e08 87 {
piroro4560 3:d3e52f43b427 88 while(1) {
piroro4560 3:d3e52f43b427 89 Button = (int)rawButton;
piroro4560 3:d3e52f43b427 90 Toggle_mode = (int)(!toggle[0])*2 + (int)(!toggle[1]);
piroro4560 3:d3e52f43b427 91 Toggle_speed = (int)(!toggle[2]);
piroro4560 3:d3e52f43b427 92
piroro4560 3:d3e52f43b427 93 PS3get(false);
piroro4560 3:d3e52f43b427 94
piroro4560 3:d3e52f43b427 95 printf("mode:%d|speed:%d | button:(%d,%d) ", Toggle_mode, Toggle_speed, Button, preButton);
piroro4560 3:d3e52f43b427 96
piroro4560 3:d3e52f43b427 97 if (Button) {
piroro4560 3:d3e52f43b427 98 switch (Toggle_mode) {
piroro4560 3:d3e52f43b427 99 case 0 :
piroro4560 3:d3e52f43b427 100 StartDemo();
piroro4560 3:d3e52f43b427 101 break;
piroro4560 3:d3e52f43b427 102 case 1 :
piroro4560 3:d3e52f43b427 103 printf("Kicker Test \r\n");
piroro4560 3:d3e52f43b427 104 Master.Shot();
piroro4560 3:d3e52f43b427 105 break;
piroro4560 3:d3e52f43b427 106 case 2 :
piroro4560 3:d3e52f43b427 107 printf("Dribbler Test \r\n");
piroro4560 3:d3e52f43b427 108 Master.Dribble(0.7);
piroro4560 3:d3e52f43b427 109 break;
piroro4560 3:d3e52f43b427 110 case 3 :
piroro4560 3:d3e52f43b427 111 MotorTest();
piroro4560 3:d3e52f43b427 112 break;
piroro4560 0:6a6e78ee4e08 113 }
piroro4560 3:d3e52f43b427 114 } else {
piroro4560 3:d3e52f43b427 115 printf("\r\n");
piroro4560 3:d3e52f43b427 116 Master.Dribble(0.0);
piroro4560 3:d3e52f43b427 117 for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
piroro4560 0:6a6e78ee4e08 118 }
piroro4560 3:d3e52f43b427 119 preButton = Button;
piroro4560 0:6a6e78ee4e08 120 }
piroro4560 0:6a6e78ee4e08 121 }
piroro4560 0:6a6e78ee4e08 122
piroro4560 0:6a6e78ee4e08 123 void PS3get(bool flag_PS3print)
piroro4560 0:6a6e78ee4e08 124 {
piroro4560 0:6a6e78ee4e08 125 h1 = mini.getHedder1();
piroro4560 0:6a6e78ee4e08 126 h2 = mini.getHedder2();
piroro4560 0:6a6e78ee4e08 127 for(int i=0; i<12; i++) {
piroro4560 0:6a6e78ee4e08 128 b[i] = mini.getButton(i);
piroro4560 0:6a6e78ee4e08 129 }
piroro4560 0:6a6e78ee4e08 130 for(int i=0; i<2; i++) {
piroro4560 0:6a6e78ee4e08 131 tr[i] = mini.getTrigger(i);
piroro4560 0:6a6e78ee4e08 132 }
piroro4560 0:6a6e78ee4e08 133 for(int i=0; i<4; i++) {
piroro4560 0:6a6e78ee4e08 134 st[i] = mini.getStick(i);
piroro4560 0:6a6e78ee4e08 135 }
piroro4560 0:6a6e78ee4e08 136 if (flag_PS3print) {
piroro4560 0:6a6e78ee4e08 137 for(int i=0; i<12; i++) printf("%d ",b[i]);
piroro4560 0:6a6e78ee4e08 138 printf("|");
piroro4560 0:6a6e78ee4e08 139 for(int i=0; i<2; i++) printf("%3d ",tr[i]);
piroro4560 0:6a6e78ee4e08 140 printf("|");
piroro4560 0:6a6e78ee4e08 141 for(int i=0; i<4; i++) printf("%3d ",st[i]);
piroro4560 0:6a6e78ee4e08 142 if(mini.getState()) printf("ok");
piroro4560 0:6a6e78ee4e08 143 else printf("bad");
piroro4560 0:6a6e78ee4e08 144 printf("\r\n");
piroro4560 0:6a6e78ee4e08 145 } else {
piroro4560 0:6a6e78ee4e08 146 mini.getState();
piroro4560 0:6a6e78ee4e08 147 }
piroro4560 3:d3e52f43b427 148 }
piroro4560 3:d3e52f43b427 149
piroro4560 5:62c9acd791ce 150 ///
piroro4560 5:62c9acd791ce 151 /// @var rawAngle
piroro4560 5:62c9acd791ce 152 /// @brief jy901の生データ
piroro4560 5:62c9acd791ce 153 /// @var targetAngle
piroro4560 5:62c9acd791ce 154 /// @brief PIDの目標角(-180 ~ 180)
piroro4560 5:62c9acd791ce 155 /// @var limitedAngle
piroro4560 5:62c9acd791ce 156 /// @brief rawAngleを(-180~180)の範囲で表す
piroro4560 5:62c9acd791ce 157 /// @var processAngle
piroro4560 5:62c9acd791ce 158 /// @brief PIDで補正される値
piroro4560 5:62c9acd791ce 159 ///
piroro4560 5:62c9acd791ce 160
piroro4560 3:d3e52f43b427 161 void StartDemo()
piroro4560 3:d3e52f43b427 162 {
piroro4560 3:d3e52f43b427 163 bool flag_Dribble=false;
piroro4560 3:d3e52f43b427 164 bool flag_Shot=false;
piroro4560 3:d3e52f43b427 165 bool flag_Rotate=false;
piroro4560 3:d3e52f43b427 166 bool b_[12];
piroro4560 3:d3e52f43b427 167
piroro4560 3:d3e52f43b427 168 double pwm[4]={};
piroro4560 3:d3e52f43b427 169 double DribblePower = 0.5;
piroro4560 3:d3e52f43b427 170 double Vx=0;
piroro4560 3:d3e52f43b427 171 double Vy=0;
piroro4560 3:d3e52f43b427 172 double Vr=0;
piroro4560 3:d3e52f43b427 173 double RotatePower=0;
piroro4560 3:d3e52f43b427 174 double V=0;
piroro4560 3:d3e52f43b427 175 double theta=0;
piroro4560 4:bbc6c27561e6 176
piroro4560 4:bbc6c27561e6 177 double rawAngle=0;
piroro4560 4:bbc6c27561e6 178 double targetAngle=0;
piroro4560 4:bbc6c27561e6 179 double limitedAngle=0;
piroro4560 4:bbc6c27561e6 180 double processAngle=0;
piroro4560 3:d3e52f43b427 181
piroro4560 3:d3e52f43b427 182 Time_Shot.start();
piroro4560 3:d3e52f43b427 183 jy.calibrateAll(50);
piroro4560 3:d3e52f43b427 184
piroro4560 3:d3e52f43b427 185 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 186 // 機体ナンバーが　１
piroro4560 3:d3e52f43b427 187 double VxLimit=0.75; // Ｘ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 188 double VyLimit=0.75; // Ｙ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 189 double VrLimit=0.25; // 回転方向ベクトルの最大値
piroro4560 3:d3e52f43b427 190 double Bias_Motor=0.75; // モーター出力値のバイアス
piroro4560 3:d3e52f43b427 191 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
piroro4560 3:d3e52f43b427 192 omni.wheel[1].setRadian(PI * 7.0 / 4.0);
piroro4560 3:d3e52f43b427 193 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
piroro4560 3:d3e52f43b427 194 omni.wheel[3].setRadian(PI * 3.0 / 4.0);
piroro4560 3:d3e52f43b427 195 #else
piroro4560 3:d3e52f43b427 196 // 機体ナンバーが　２
piroro4560 3:d3e52f43b427 197 double VxLimit=0.8; // Ｘ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 198 double VyLimit=0.8; // Ｙ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 199 double VrLimit=0.3; // 回転方向ベクトルの最大値
piroro4560 3:d3e52f43b427 200 double Bias_Motor=0.8; // モーター出力値のバイアス
piroro4560 3:d3e52f43b427 201 double Bias_DefectiveMD = 0.6; // ダメダメＭＤ補正用バイアス
piroro4560 3:d3e52f43b427 202 omni.wheel[0].setRadian(PI * 5.0 / 4.0);
piroro4560 3:d3e52f43b427 203 omni.wheel[1].setRadian(PI * 7.0 / 4.0);
piroro4560 3:d3e52f43b427 204 omni.wheel[2].setRadian(PI * 1.0 / 4.0);
piroro4560 3:d3e52f43b427 205 omni.wheel[3].setRadian(PI * 3.0 / 4.0);
piroro4560 3:d3e52f43b427 206 #endif
piroro4560 3:d3e52f43b427 207
piroro4560 3:d3e52f43b427 208 Rotate.setInputLimits(-360, 360);
piroro4560 3:d3e52f43b427 209 Rotate.setOutputLimits(-1*VrLimit,VrLimit);
piroro4560 3:d3e52f43b427 210 Rotate.setBias(0);
piroro4560 3:d3e52f43b427 211 Rotate.setMode(1);
piroro4560 3:d3e52f43b427 212 Rotate.setSetPoint(targetAngle);
piroro4560 3:d3e52f43b427 213
piroro4560 3:d3e52f43b427 214 // 以下ループ
piroro4560 3:d3e52f43b427 215 while (true) {
piroro4560 3:d3e52f43b427 216 PS3get(true);
piroro4560 3:d3e52f43b427 217 Button = rawButton;
piroro4560 3:d3e52f43b427 218
piroro4560 3:d3e52f43b427 219 // UIボードの処理
piroro4560 3:d3e52f43b427 220 Toggle_mode = (int)(!toggle[0])*2 + (int)(!toggle[1]);
piroro4560 3:d3e52f43b427 221 Toggle_speed = (int)(!toggle[2]);
piroro4560 3:d3e52f43b427 222 if (!Button && preButton) {
piroro4560 3:d3e52f43b427 223 if (!flag_Passfunc) {
piroro4560 3:d3e52f43b427 224 flag_Passfunc = true;
piroro4560 3:d3e52f43b427 225 } else {
piroro4560 3:d3e52f43b427 226 Toggle_mode = 4;
piroro4560 3:d3e52f43b427 227 }
piroro4560 3:d3e52f43b427 228 }
piroro4560 3:d3e52f43b427 229 if (Toggle_mode!=0) {
piroro4560 3:d3e52f43b427 230 for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
piroro4560 3:d3e52f43b427 231 Master.Dribble(0.0);
piroro4560 3:d3e52f43b427 232 Time_MotorTest.reset();
piroro4560 3:d3e52f43b427 233 Time_MotorTest.stop();
piroro4560 3:d3e52f43b427 234 flag_Passfunc = false;
piroro4560 3:d3e52f43b427 235 return;
piroro4560 3:d3e52f43b427 236 }
piroro4560 3:d3e52f43b427 237
piroro4560 3:d3e52f43b427 238 // 以下ペアリング中のループ
piroro4560 3:d3e52f43b427 239 if (mini.getState()) {
piroro4560 3:d3e52f43b427 240
piroro4560 3:d3e52f43b427 241 // ジャイロ
piroro4560 3:d3e52f43b427 242 rawAngle = jy.getZaxisAngle();
piroro4560 3:d3e52f43b427 243 if (rawAngle > 180.0) rawAngle -= 360;
piroro4560 3:d3e52f43b427 244 if (rawAngle < -180.0) rawAngle += 360;
piroro4560 3:d3e52f43b427 245 limitedAngle = rawAngle;
piroro4560 3:d3e52f43b427 246
piroro4560 3:d3e52f43b427 247 // 右スティックで回転
piroro4560 3:d3e52f43b427 248 if (fabs(st[2]-128.0) > 30) {
piroro4560 3:d3e52f43b427 249 targetAngle = limitedAngle;
piroro4560 3:d3e52f43b427 250 flag_Rotate = true;
piroro4560 3:d3e52f43b427 251 } else {
piroro4560 3:d3e52f43b427 252 flag_Rotate = false;
piroro4560 3:d3e52f43b427 253 }
piroro4560 3:d3e52f43b427 254
piroro4560 3:d3e52f43b427 255 if ((st[3] > 220) && b[10] && !b_[10]) {
piroro4560 3:d3e52f43b427 256 targetAngle = fmod((targetAngle + 180), 360);
piroro4560 3:d3e52f43b427 257 }
piroro4560 3:d3e52f43b427 258
piroro4560 3:d3e52f43b427 259 // 零点出し
piroro4560 3:d3e52f43b427 260 if (b[1]) {
piroro4560 3:d3e52f43b427 261 jy.yawcalibrate();
piroro4560 3:d3e52f43b427 262 Rotate.setSetPoint(0);
piroro4560 3:d3e52f43b427 263 targetAngle = 0;
piroro4560 3:d3e52f43b427 264 }
piroro4560 3:d3e52f43b427 265
piroro4560 3:d3e52f43b427 266 // PIDの調整
piroro4560 3:d3e52f43b427 267 processAngle = limitedAngle - targetAngle;
piroro4560 3:d3e52f43b427 268 if (processAngle > 180.0) processAngle = processAngle - 360.0;
piroro4560 3:d3e52f43b427 269 if (processAngle < -180.0) processAngle = processAngle + 360.0;
piroro4560 3:d3e52f43b427 270 Rotate.setProcessValue(processAngle);
piroro4560 3:d3e52f43b427 271 RotatePower = Rotate.compute();
piroro4560 3:d3e52f43b427 272
piroro4560 3:d3e52f43b427 273 // モーター出力値の計算
piroro4560 3:d3e52f43b427 274 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 275 // 機体ナンバーが　１
piroro4560 3:d3e52f43b427 276 Vx = ((st[0]-128.0)/128.0) * VxLimit;
piroro4560 3:d3e52f43b427 277 Vy = (128.0-st[1])/128.0 * VyLimit;
piroro4560 3:d3e52f43b427 278 Vr = -(st[2]-128.0)/128.0 * VrLimit;
piroro4560 3:d3e52f43b427 279 #else
piroro4560 3:d3e52f43b427 280 // 機体ナンバーが　２
piroro4560 3:d3e52f43b427 281 Vx = ((st[0]-128.0)/128.0) * VxLimit;
piroro4560 3:d3e52f43b427 282 Vy = (128.0-st[1])/128.0 * VyLimit;
piroro4560 3:d3e52f43b427 283 Vr = -(st[2]-128.0)/128.0 * VrLimit;
piroro4560 3:d3e52f43b427 284 #endif
piroro4560 3:d3e52f43b427 285
piroro4560 3:d3e52f43b427 286 if (!flag_Rotate) {
piroro4560 3:d3e52f43b427 287 Vr = RotatePower;
piroro4560 3:d3e52f43b427 288 }
piroro4560 3:d3e52f43b427 289
piroro4560 3:d3e52f43b427 290 V = hypot(Vx, Vy);
piroro4560 3:d3e52f43b427 291 if (!Toggle_speed) V *= 0.5;
piroro4560 3:d3e52f43b427 292 theta = atan2(Vy, Vx);
piroro4560 3:d3e52f43b427 293 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 294 omni.computeCircular(V, theta - limitedAngle/180.0*PI, Vr);
piroro4560 3:d3e52f43b427 295 #else
piroro4560 3:d3e52f43b427 296 omni.computeCircular(V, theta - limitedAngle/180.0*PI, -Vr);
piroro4560 3:d3e52f43b427 297 #endif
piroro4560 3:d3e52f43b427 298 for(int i=0; i < 4; i++) {
piroro4560 3:d3e52f43b427 299 pwm[i] = ((double)omni.wheel[i] * Bias_Motor);
piroro4560 3:d3e52f43b427 300 }
piroro4560 3:d3e52f43b427 301
piroro4560 3:d3e52f43b427 302 // フラグ処理
piroro4560 3:d3e52f43b427 303 if (b[9]) flag_Dribble = true;
piroro4560 3:d3e52f43b427 304 else flag_Dribble = false;
piroro4560 3:d3e52f43b427 305
piroro4560 3:d3e52f43b427 306 if (b[8] && (Time_Shot.read_ms() > 1000)) {
piroro4560 3:d3e52f43b427 307 flag_Shot = true;
piroro4560 3:d3e52f43b427 308 Time_Shot.reset();
piroro4560 3:d3e52f43b427 309 } else {
piroro4560 3:d3e52f43b427 310 flag_Shot = false;
piroro4560 3:d3e52f43b427 311 }
piroro4560 3:d3e52f43b427 312
piroro4560 3:d3e52f43b427 313 // ドリブラー・キッカー
piroro4560 3:d3e52f43b427 314 if (flag_Dribble) Master.Dribble(0.7);
piroro4560 3:d3e52f43b427 315 else Master.Dribble(0.0);
piroro4560 3:d3e52f43b427 316 if (flag_Shot) Master.Shot();
piroro4560 3:d3e52f43b427 317
piroro4560 3:d3e52f43b427 318 // ニュートラル処理
piroro4560 3:d3e52f43b427 319 for (int i=0; i<4; i++) {
piroro4560 3:d3e52f43b427 320 if (fabs(pwm[i]) < 0.05) pwm[i] = 0.0;
piroro4560 3:d3e52f43b427 321 }
piroro4560 3:d3e52f43b427 322
piroro4560 3:d3e52f43b427 323 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 324 Master.SetValueMotor(0, pwm[0]);
piroro4560 3:d3e52f43b427 325 Master.SetValueMotor(1, pwm[1]);
piroro4560 3:d3e52f43b427 326 Master.SetValueMotor(2, -pwm[2]);
piroro4560 3:d3e52f43b427 327 Master.SetValueMotor(3, -pwm[3]);
piroro4560 3:d3e52f43b427 328 #else
piroro4560 3:d3e52f43b427 329 Master.SetValueMotor(0, pwm[0] * Bias_DefectiveMD);
piroro4560 3:d3e52f43b427 330 Master.SetValueMotor(1, pwm[1]);
piroro4560 3:d3e52f43b427 331 Master.SetValueMotor(2, pwm[2]);
piroro4560 3:d3e52f43b427 332 Master.SetValueMotor(3, pwm[3] * Bias_DefectiveMD);
piroro4560 3:d3e52f43b427 333 #endif
piroro4560 3:d3e52f43b427 334
piroro4560 3:d3e52f43b427 335 printf("(Vx,Vy, Vr):(%3d,%3d,%3d)|theta:%d | angle:%d | speed:%d | "
piroro4560 3:d3e52f43b427 336 , (int)(Vx*100), (int)(Vy*100), (int)(Vr*100), (int)(theta), (int)limitedAngle, Toggle_speed);
piroro4560 3:d3e52f43b427 337 }
piroro4560 3:d3e52f43b427 338
piroro4560 3:d3e52f43b427 339 // １ループ前のボタンの値
piroro4560 3:d3e52f43b427 340 preButton = Button;
piroro4560 3:d3e52f43b427 341 for (int i=0; i<12; i++) b_[i] = b[i];
piroro4560 3:d3e52f43b427 342 }
piroro4560 3:d3e52f43b427 343 }
piroro4560 3:d3e52f43b427 344
piroro4560 3:d3e52f43b427 345 void MotorTest()
piroro4560 3:d3e52f43b427 346 {
piroro4560 3:d3e52f43b427 347 Time_MotorTest.start();
piroro4560 3:d3e52f43b427 348 uint8_t Num_MotorTest = 0;
piroro4560 3:d3e52f43b427 349 double pwm=0.0;
piroro4560 3:d3e52f43b427 350
piroro4560 3:d3e52f43b427 351 while(1) {
piroro4560 3:d3e52f43b427 352 Button = rawButton;
piroro4560 3:d3e52f43b427 353 pwm = sin(Time_MotorTest.read() * PI);
piroro4560 3:d3e52f43b427 354 Master.SetValueMotor(Num_MotorTest, pwm);
piroro4560 3:d3e52f43b427 355 if (Time_MotorTest.read() > 2.0) {
piroro4560 3:d3e52f43b427 356 Master.SetValueMotor(Num_MotorTest, 0.0);
piroro4560 3:d3e52f43b427 357 Num_MotorTest = (Num_MotorTest+1) % 4;
piroro4560 3:d3e52f43b427 358 Time_MotorTest.reset();
piroro4560 3:d3e52f43b427 359 }
piroro4560 3:d3e52f43b427 360 if (!Button && preButton) {
piroro4560 3:d3e52f43b427 361 if (!flag_Passfunc) {
piroro4560 3:d3e52f43b427 362 flag_Passfunc = true;
piroro4560 3:d3e52f43b427 363 } else {
piroro4560 3:d3e52f43b427 364 for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
piroro4560 3:d3e52f43b427 365 Time_MotorTest.reset();
piroro4560 3:d3e52f43b427 366 Time_MotorTest.stop();
piroro4560 3:d3e52f43b427 367 flag_Passfunc = false;
piroro4560 3:d3e52f43b427 368 return;
piroro4560 3:d3e52f43b427 369 }
piroro4560 3:d3e52f43b427 370 }
piroro4560 3:d3e52f43b427 371 printf("Motor Test | motor%d:%3d | button:(%d,%d) | \r\n"
piroro4560 3:d3e52f43b427 372 , Num_MotorTest, (int)((pwm)*100), (int)Button, preButton);
piroro4560 3:d3e52f43b427 373 preButton = Button;
piroro4560 3:d3e52f43b427 374 }
piroro4560 0:6a6e78ee4e08 375 }