Quadrifoglio / Mbed OS 300_PS3com

Communicate with PS3 controller

Dependencies:   omni_wheel PID jy901 solenoid Master kohiMD lpf SerialArduino RCJESC

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

Committer:
piroro4560
Date:
Thu Jun 23 15:06:31 2022 +0000
Revision:
6:cb4a0fc3f90c
Parent:
5:232c0cfdec93
Child:
7:9b4acb93f94d
Edit doxygen

Who changed what in which revision?

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