Quadrifoglio / Mbed OS 300_PS3com

Communicate with PS3 controller

Dependencies:   omni_wheel PID jy901 solenoid Master kohiMD lpf SerialArduino RCJESC

main.cpp@7:9b4acb93f94d, 2022-06-23 (annotated)

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