Ryu Anzawa / DoxygenTest2

doxygen test

main.cpp@6:ac8204439a89, 2022-06-23 (annotated)

Committer:
piroro4560
Date:
Thu Jun 23 15:18:39 2022 +0000
Revision:
6:ac8204439a89
Parent:
5:62c9acd791ce 
test6

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 6:ac8204439a89 150
piroro4560 5:62c9acd791ce 151
piroro4560 3:d3e52f43b427 152 void StartDemo()
piroro4560 3:d3e52f43b427 153 {
piroro4560 3:d3e52f43b427 154 bool flag_Dribble=false;
piroro4560 3:d3e52f43b427 155 bool flag_Shot=false;
piroro4560 3:d3e52f43b427 156 bool flag_Rotate=false;
piroro4560 3:d3e52f43b427 157 bool b_[12];
piroro4560 3:d3e52f43b427 158
piroro4560 3:d3e52f43b427 159 double pwm[4]={};
piroro4560 3:d3e52f43b427 160 double DribblePower = 0.5;
piroro4560 3:d3e52f43b427 161 double Vx=0;
piroro4560 3:d3e52f43b427 162 double Vy=0;
piroro4560 3:d3e52f43b427 163 double Vr=0;
piroro4560 3:d3e52f43b427 164 double RotatePower=0;
piroro4560 3:d3e52f43b427 165 double V=0;
piroro4560 3:d3e52f43b427 166 double theta=0;
piroro4560 4:bbc6c27561e6 167
piroro4560 6:ac8204439a89 168 /// @var rawAngle
piroro4560 6:ac8204439a89 169 /// @brief jy901の生データ
piroro4560 6:ac8204439a89 170 /// @var targetAngle
piroro4560 6:ac8204439a89 171 /// @brief PIDの目標角(-180 ~ 180)
piroro4560 6:ac8204439a89 172 /// @var limitedAngle
piroro4560 6:ac8204439a89 173 /// @brief rawAngleを(-180~180)の範囲で表す
piroro4560 6:ac8204439a89 174 /// @var processAngle
piroro4560 6:ac8204439a89 175 /// @brief PIDで補正される値
piroro4560 4:bbc6c27561e6 176 double rawAngle=0;
piroro4560 4:bbc6c27561e6 177 double targetAngle=0;
piroro4560 4:bbc6c27561e6 178 double limitedAngle=0;
piroro4560 4:bbc6c27561e6 179 double processAngle=0;
piroro4560 3:d3e52f43b427 180
piroro4560 3:d3e52f43b427 181 Time_Shot.start();
piroro4560 3:d3e52f43b427 182 jy.calibrateAll(50);
piroro4560 3:d3e52f43b427 183
piroro4560 3:d3e52f43b427 184 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 185 // 機体ナンバーが　１
piroro4560 3:d3e52f43b427 186 double VxLimit=0.75; // Ｘ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 187 double VyLimit=0.75; // Ｙ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 188 double VrLimit=0.25; // 回転方向ベクトルの最大値
piroro4560 3:d3e52f43b427 189 double Bias_Motor=0.75; // モーター出力値のバイアス
piroro4560 3:d3e52f43b427 190 omni.wheel[0].setRadian(PI * 1.0 / 4.0);
piroro4560 3:d3e52f43b427 191 omni.wheel[1].setRadian(PI * 7.0 / 4.0);
piroro4560 3:d3e52f43b427 192 omni.wheel[2].setRadian(PI * 5.0 / 4.0);
piroro4560 3:d3e52f43b427 193 omni.wheel[3].setRadian(PI * 3.0 / 4.0);
piroro4560 3:d3e52f43b427 194 #else
piroro4560 3:d3e52f43b427 195 // 機体ナンバーが　２
piroro4560 3:d3e52f43b427 196 double VxLimit=0.8; // Ｘ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 197 double VyLimit=0.8; // Ｙ軸方向ベクトルの最大値
piroro4560 3:d3e52f43b427 198 double VrLimit=0.3; // 回転方向ベクトルの最大値
piroro4560 3:d3e52f43b427 199 double Bias_Motor=0.8; // モーター出力値のバイアス
piroro4560 3:d3e52f43b427 200 double Bias_DefectiveMD = 0.6; // ダメダメＭＤ補正用バイアス
piroro4560 3:d3e52f43b427 201 omni.wheel[0].setRadian(PI * 5.0 / 4.0);
piroro4560 3:d3e52f43b427 202 omni.wheel[1].setRadian(PI * 7.0 / 4.0);
piroro4560 3:d3e52f43b427 203 omni.wheel[2].setRadian(PI * 1.0 / 4.0);
piroro4560 3:d3e52f43b427 204 omni.wheel[3].setRadian(PI * 3.0 / 4.0);
piroro4560 3:d3e52f43b427 205 #endif
piroro4560 3:d3e52f43b427 206
piroro4560 3:d3e52f43b427 207 Rotate.setInputLimits(-360, 360);
piroro4560 3:d3e52f43b427 208 Rotate.setOutputLimits(-1*VrLimit,VrLimit);
piroro4560 3:d3e52f43b427 209 Rotate.setBias(0);
piroro4560 3:d3e52f43b427 210 Rotate.setMode(1);
piroro4560 3:d3e52f43b427 211 Rotate.setSetPoint(targetAngle);
piroro4560 3:d3e52f43b427 212
piroro4560 3:d3e52f43b427 213 // 以下ループ
piroro4560 3:d3e52f43b427 214 while (true) {
piroro4560 3:d3e52f43b427 215 PS3get(true);
piroro4560 3:d3e52f43b427 216 Button = rawButton;
piroro4560 3:d3e52f43b427 217
piroro4560 3:d3e52f43b427 218 // UIボードの処理
piroro4560 3:d3e52f43b427 219 Toggle_mode = (int)(!toggle[0])*2 + (int)(!toggle[1]);
piroro4560 3:d3e52f43b427 220 Toggle_speed = (int)(!toggle[2]);
piroro4560 3:d3e52f43b427 221 if (!Button && preButton) {
piroro4560 3:d3e52f43b427 222 if (!flag_Passfunc) {
piroro4560 3:d3e52f43b427 223 flag_Passfunc = true;
piroro4560 3:d3e52f43b427 224 } else {
piroro4560 3:d3e52f43b427 225 Toggle_mode = 4;
piroro4560 3:d3e52f43b427 226 }
piroro4560 3:d3e52f43b427 227 }
piroro4560 3:d3e52f43b427 228 if (Toggle_mode!=0) {
piroro4560 3:d3e52f43b427 229 for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
piroro4560 3:d3e52f43b427 230 Master.Dribble(0.0);
piroro4560 3:d3e52f43b427 231 Time_MotorTest.reset();
piroro4560 3:d3e52f43b427 232 Time_MotorTest.stop();
piroro4560 3:d3e52f43b427 233 flag_Passfunc = false;
piroro4560 3:d3e52f43b427 234 return;
piroro4560 3:d3e52f43b427 235 }
piroro4560 3:d3e52f43b427 236
piroro4560 3:d3e52f43b427 237 // 以下ペアリング中のループ
piroro4560 3:d3e52f43b427 238 if (mini.getState()) {
piroro4560 3:d3e52f43b427 239
piroro4560 3:d3e52f43b427 240 // ジャイロ
piroro4560 3:d3e52f43b427 241 rawAngle = jy.getZaxisAngle();
piroro4560 3:d3e52f43b427 242 if (rawAngle > 180.0) rawAngle -= 360;
piroro4560 3:d3e52f43b427 243 if (rawAngle < -180.0) rawAngle += 360;
piroro4560 3:d3e52f43b427 244 limitedAngle = rawAngle;
piroro4560 3:d3e52f43b427 245
piroro4560 3:d3e52f43b427 246 // 右スティックで回転
piroro4560 3:d3e52f43b427 247 if (fabs(st[2]-128.0) > 30) {
piroro4560 3:d3e52f43b427 248 targetAngle = limitedAngle;
piroro4560 3:d3e52f43b427 249 flag_Rotate = true;
piroro4560 3:d3e52f43b427 250 } else {
piroro4560 3:d3e52f43b427 251 flag_Rotate = false;
piroro4560 3:d3e52f43b427 252 }
piroro4560 3:d3e52f43b427 253
piroro4560 3:d3e52f43b427 254 if ((st[3] > 220) && b[10] && !b_[10]) {
piroro4560 3:d3e52f43b427 255 targetAngle = fmod((targetAngle + 180), 360);
piroro4560 3:d3e52f43b427 256 }
piroro4560 3:d3e52f43b427 257
piroro4560 3:d3e52f43b427 258 // 零点出し
piroro4560 3:d3e52f43b427 259 if (b[1]) {
piroro4560 3:d3e52f43b427 260 jy.yawcalibrate();
piroro4560 3:d3e52f43b427 261 Rotate.setSetPoint(0);
piroro4560 3:d3e52f43b427 262 targetAngle = 0;
piroro4560 3:d3e52f43b427 263 }
piroro4560 3:d3e52f43b427 264
piroro4560 3:d3e52f43b427 265 // PIDの調整
piroro4560 3:d3e52f43b427 266 processAngle = limitedAngle - targetAngle;
piroro4560 3:d3e52f43b427 267 if (processAngle > 180.0) processAngle = processAngle - 360.0;
piroro4560 3:d3e52f43b427 268 if (processAngle < -180.0) processAngle = processAngle + 360.0;
piroro4560 3:d3e52f43b427 269 Rotate.setProcessValue(processAngle);
piroro4560 3:d3e52f43b427 270 RotatePower = Rotate.compute();
piroro4560 3:d3e52f43b427 271
piroro4560 3:d3e52f43b427 272 // モーター出力値の計算
piroro4560 3:d3e52f43b427 273 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 274 // 機体ナンバーが　１
piroro4560 3:d3e52f43b427 275 Vx = ((st[0]-128.0)/128.0) * VxLimit;
piroro4560 3:d3e52f43b427 276 Vy = (128.0-st[1])/128.0 * VyLimit;
piroro4560 3:d3e52f43b427 277 Vr = -(st[2]-128.0)/128.0 * VrLimit;
piroro4560 3:d3e52f43b427 278 #else
piroro4560 3:d3e52f43b427 279 // 機体ナンバーが　２
piroro4560 3:d3e52f43b427 280 Vx = ((st[0]-128.0)/128.0) * VxLimit;
piroro4560 3:d3e52f43b427 281 Vy = (128.0-st[1])/128.0 * VyLimit;
piroro4560 3:d3e52f43b427 282 Vr = -(st[2]-128.0)/128.0 * VrLimit;
piroro4560 3:d3e52f43b427 283 #endif
piroro4560 3:d3e52f43b427 284
piroro4560 3:d3e52f43b427 285 if (!flag_Rotate) {
piroro4560 3:d3e52f43b427 286 Vr = RotatePower;
piroro4560 3:d3e52f43b427 287 }
piroro4560 3:d3e52f43b427 288
piroro4560 3:d3e52f43b427 289 V = hypot(Vx, Vy);
piroro4560 3:d3e52f43b427 290 if (!Toggle_speed) V *= 0.5;
piroro4560 3:d3e52f43b427 291 theta = atan2(Vy, Vx);
piroro4560 3:d3e52f43b427 292 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 293 omni.computeCircular(V, theta - limitedAngle/180.0*PI, Vr);
piroro4560 3:d3e52f43b427 294 #else
piroro4560 3:d3e52f43b427 295 omni.computeCircular(V, theta - limitedAngle/180.0*PI, -Vr);
piroro4560 3:d3e52f43b427 296 #endif
piroro4560 3:d3e52f43b427 297 for(int i=0; i < 4; i++) {
piroro4560 3:d3e52f43b427 298 pwm[i] = ((double)omni.wheel[i] * Bias_Motor);
piroro4560 3:d3e52f43b427 299 }
piroro4560 3:d3e52f43b427 300
piroro4560 3:d3e52f43b427 301 // フラグ処理
piroro4560 3:d3e52f43b427 302 if (b[9]) flag_Dribble = true;
piroro4560 3:d3e52f43b427 303 else flag_Dribble = false;
piroro4560 3:d3e52f43b427 304
piroro4560 3:d3e52f43b427 305 if (b[8] && (Time_Shot.read_ms() > 1000)) {
piroro4560 3:d3e52f43b427 306 flag_Shot = true;
piroro4560 3:d3e52f43b427 307 Time_Shot.reset();
piroro4560 3:d3e52f43b427 308 } else {
piroro4560 3:d3e52f43b427 309 flag_Shot = false;
piroro4560 3:d3e52f43b427 310 }
piroro4560 3:d3e52f43b427 311
piroro4560 3:d3e52f43b427 312 // ドリブラー・キッカー
piroro4560 3:d3e52f43b427 313 if (flag_Dribble) Master.Dribble(0.7);
piroro4560 3:d3e52f43b427 314 else Master.Dribble(0.0);
piroro4560 3:d3e52f43b427 315 if (flag_Shot) Master.Shot();
piroro4560 3:d3e52f43b427 316
piroro4560 3:d3e52f43b427 317 // ニュートラル処理
piroro4560 3:d3e52f43b427 318 for (int i=0; i<4; i++) {
piroro4560 3:d3e52f43b427 319 if (fabs(pwm[i]) < 0.05) pwm[i] = 0.0;
piroro4560 3:d3e52f43b427 320 }
piroro4560 3:d3e52f43b427 321
piroro4560 3:d3e52f43b427 322 #ifdef RobotNumberIsOne
piroro4560 3:d3e52f43b427 323 Master.SetValueMotor(0, pwm[0]);
piroro4560 3:d3e52f43b427 324 Master.SetValueMotor(1, pwm[1]);
piroro4560 3:d3e52f43b427 325 Master.SetValueMotor(2, -pwm[2]);
piroro4560 3:d3e52f43b427 326 Master.SetValueMotor(3, -pwm[3]);
piroro4560 3:d3e52f43b427 327 #else
piroro4560 3:d3e52f43b427 328 Master.SetValueMotor(0, pwm[0] * Bias_DefectiveMD);
piroro4560 3:d3e52f43b427 329 Master.SetValueMotor(1, pwm[1]);
piroro4560 3:d3e52f43b427 330 Master.SetValueMotor(2, pwm[2]);
piroro4560 3:d3e52f43b427 331 Master.SetValueMotor(3, pwm[3] * Bias_DefectiveMD);
piroro4560 3:d3e52f43b427 332 #endif
piroro4560 3:d3e52f43b427 333
piroro4560 3:d3e52f43b427 334 printf("(Vx,Vy, Vr):(%3d,%3d,%3d)|theta:%d | angle:%d | speed:%d | "
piroro4560 3:d3e52f43b427 335 , (int)(Vx*100), (int)(Vy*100), (int)(Vr*100), (int)(theta), (int)limitedAngle, Toggle_speed);
piroro4560 3:d3e52f43b427 336 }
piroro4560 3:d3e52f43b427 337
piroro4560 3:d3e52f43b427 338 // １ループ前のボタンの値
piroro4560 3:d3e52f43b427 339 preButton = Button;
piroro4560 3:d3e52f43b427 340 for (int i=0; i<12; i++) b_[i] = b[i];
piroro4560 3:d3e52f43b427 341 }
piroro4560 3:d3e52f43b427 342 }
piroro4560 3:d3e52f43b427 343
piroro4560 3:d3e52f43b427 344 void MotorTest()
piroro4560 3:d3e52f43b427 345 {
piroro4560 3:d3e52f43b427 346 Time_MotorTest.start();
piroro4560 3:d3e52f43b427 347 uint8_t Num_MotorTest = 0;
piroro4560 3:d3e52f43b427 348 double pwm=0.0;
piroro4560 3:d3e52f43b427 349
piroro4560 3:d3e52f43b427 350 while(1) {
piroro4560 3:d3e52f43b427 351 Button = rawButton;
piroro4560 3:d3e52f43b427 352 pwm = sin(Time_MotorTest.read() * PI);
piroro4560 3:d3e52f43b427 353 Master.SetValueMotor(Num_MotorTest, pwm);
piroro4560 3:d3e52f43b427 354 if (Time_MotorTest.read() > 2.0) {
piroro4560 3:d3e52f43b427 355 Master.SetValueMotor(Num_MotorTest, 0.0);
piroro4560 3:d3e52f43b427 356 Num_MotorTest = (Num_MotorTest+1) % 4;
piroro4560 3:d3e52f43b427 357 Time_MotorTest.reset();
piroro4560 3:d3e52f43b427 358 }
piroro4560 3:d3e52f43b427 359 if (!Button && preButton) {
piroro4560 3:d3e52f43b427 360 if (!flag_Passfunc) {
piroro4560 3:d3e52f43b427 361 flag_Passfunc = true;
piroro4560 3:d3e52f43b427 362 } else {
piroro4560 3:d3e52f43b427 363 for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0);
piroro4560 3:d3e52f43b427 364 Time_MotorTest.reset();
piroro4560 3:d3e52f43b427 365 Time_MotorTest.stop();
piroro4560 3:d3e52f43b427 366 flag_Passfunc = false;
piroro4560 3:d3e52f43b427 367 return;
piroro4560 3:d3e52f43b427 368 }
piroro4560 3:d3e52f43b427 369 }
piroro4560 3:d3e52f43b427 370 printf("Motor Test | motor%d:%3d | button:(%d,%d) | \r\n"
piroro4560 3:d3e52f43b427 371 , Num_MotorTest, (int)((pwm)*100), (int)Button, preButton);
piroro4560 3:d3e52f43b427 372 preButton = Button;
piroro4560 3:d3e52f43b427 373 }
piroro4560 0:6a6e78ee4e08 374 }