doxygen test
Diff: main.cpp
- Revision:
- 3:d3e52f43b427
- Parent:
- 2:98b9c6e44ae4
- Child:
- 4:bbc6c27561e6
diff -r 98b9c6e44ae4 -r d3e52f43b427 main.cpp --- a/main.cpp Sun Jun 19 07:34:09 2022 +0000 +++ b/main.cpp Thu Jun 23 15:10:41 2022 +0000 @@ -1,3 +1,4 @@ +/// /// @file main.cpp /// @brief ロボカップラジコンのプログラム /// @@ -7,193 +8,115 @@ /// @note 1:速め 0:遅め /// /// @details 中央トグル2つでモード切替 -/// @note 中央トグルでのモード切替; -/// 左0右0:スタート -/// 左1右0:キッカーテスト -/// 左0右1:ドリブラーテスト -/// 左1右1:オムニテスト +/// @note Refer +/// - 中央トグルでのモード切替; +/// - 左0右0:スタート +/// - 左1右0:キッカーテスト +/// - 左0右1:ドリブラーテスト +/// - 左1右1:オムニテスト +/// - ボタンを押すかトグルスイッチを変えるとワイヤレスモード中止 /// /// @details スタート後にPSボタンを押してペアリング 以下操作方法 -/// @note 左スティックで移動(絶対角) -/// 右スティックX軸で回転 -/// 〇でドリブル、△でシュート +/// @note Refer +/// - 左スティックで移動(絶対角) +/// - 右スティックX軸で回転 +/// - 〇でドリブル、△でシュート /// /// @attention 1番機は起動後にArduinoのリセットボタンを押す必要あり /// - -//--------操作方法-------- - - #include "Master.h" #include "jy901.h" #include "PID.h" +#include "omni_wheel.h" #include "SerialArduino.h" DigitalOut led(LED1); -Master Master;// 出力等の基本クラス +DigitalIn toggle[3] = { + DigitalIn(Pin_toggle_0), + DigitalIn(Pin_toggle_1), + DigitalIn(Pin_toggle_2) +}; + +SerialArduino mini(Pin_Arduino_TX, Pin_Arduino_RX, 115200); +Master Master; +JY901 jy(Pin_JY901_sda, Pin_JY901_scl); +DigitalIn rawButton(Pin_button); OmniWheel omni(4); -SerialArduino mini(A0, A1, 115200);// Arduinoを用いてPS3コントローラと通信するクラス -JY901 jy(jysda, jyscl); +Timer Time_Shot; +Timer Time_MotorTest; + #ifdef RobotNumberIsOne -PID Rotate(6.0, 50.0, 0.0005, 0.01); +PID Rotate(25.0, 100.0, 0.0001, 0.01); #else -PID Rotate(5.0, 50.0, 0.0005, 0.01); +PID Rotate(15.0, 100.0, 0.0001, 0.01); #endif -Timer Time_Shot; + +/// +/// @fn PS3get(bool flag_PS3print) +/// @brief ループ毎に実行。SerialArduinoライブラリを用いてPS3コントローラの値を取得。 +/// @param flag_PS3print 受け取った値を表示したい時にtrue +/// void PS3get(bool flag_PS3print); +/// +/// @fn StartDemo(); +/// DualShock3を用いたラジコン操作 +/// @brief modeが0の時にボタンを押すとスタート +/// @attention ロボットの機体番号に応じて出力とピンを変える必要アリ +/// +void StartDemo(); + +/// +/// @fn MotorTest() +/// @brief モーターを4つ連続で回す。1秒で正転反転が切り替わる +/// @brief modeが2の時にボタンを押すとスタート +/// +void MotorTest(); + +bool flag_Passfunc; +uint8_t preButton; +uint8_t Button; +uint8_t Toggle_mode=0, Toggle_speed=0; uint8_t h1,h2; -bool b[12]= {}; +bool b[12]= {}; uint8_t st[4]= {}; uint8_t tr[2]= {}; int main() { - uint8_t mode=0; - bool EdgeToggle[3]={}, pretoggle[3]={}; - bool flag_Dribble=false, flag_Shot=false, flag_Rotate; - Time_Shot.start(); - - double pwm[4]={}, DribblePower = 0.5; - double Vx=0, Vy=0, Vr=0, Rotate_Power=0, V=0, theta=0; - double rawAngle, angleLimit; - - Master.SetPS3Address(b, tr, st); - jy.calibrateAll(50); - -// DisplayThread.start(Display); -#ifdef RobotNumberIsOne -// 機体ナンバーが 1 - double VxLimit=0.75; // X軸方向ベクトルの最大値 - double VyLimit=0.75; // Y軸方向ベクトルの最大値 - double VrLimit=0.4; // 回転方向ベクトルの最大値 - double Bias_Motor=0.75; // モーター出力値のバイアス - omni.wheel[0].setRadian(PI * 1.0 / 4.0); - omni.wheel[1].setRadian(PI * 7.0 / 4.0); - omni.wheel[2].setRadian(PI * 5.0 / 4.0); - omni.wheel[3].setRadian(PI * 3.0 / 4.0); -#else -// 機体ナンバーが 2 - double VxLimit=0.8; // X軸方向ベクトルの最大値 - double VyLimit=0.8; // Y軸方向ベクトルの最大値 - double VrLimit=0.6; // 回転方向ベクトルの最大値 - double Bias_Motor=0.8; // モーター出力値のバイアス - double Bias_DefectiveMD = 0.6; // ダメダメMD補正用バイアス - omni.wheel[0].setRadian(PI * 5.0 / 4.0); - omni.wheel[1].setRadian(PI * 7.0 / 4.0); - omni.wheel[2].setRadian(PI * 1.0 / 4.0); - omni.wheel[3].setRadian(PI * 3.0 / 4.0); -#endif - - Rotate.setInputLimits(-180, 180); - Rotate.setOutputLimits(-1*VrLimit,VrLimit); - Rotate.setBias(0); - Rotate.setMode(1); - Rotate.setSetPoint(0); - -// 以下ループ - while (true) { - if (mini.getState()) { - rawAngle = jy.getZaxisAngle(); - if(rawAngle > 180.0) rawAngle -= 360; - - angleLimit = rawAngle; - Rotate.setProcessValue(angleLimit/*-sensor_.ballAngle*/); - Rotate_Power = Rotate.compute(); - - PS3get(true); - - // printf("angle : %d|power : %d | ", (int)rawAngle, (int)(Rotate_Power*100)); - printf("(Vx,Vy):(%3d,%3d)|theta:%d|angle:%d | ", (int)(Vx*100), (int)(Vy*100), (int)(theta), (int)angleLimit); - - if (fabs(st[2]-128.0) > 50) { - Rotate.setSetPoint(angleLimit); - flag_Rotate = true; - } else { - flag_Rotate = false; - } - - if (b[1]) { - jy.yawcalibrate(); - Rotate.setSetPoint(0); + while(1) { + Button = (int)rawButton; + Toggle_mode = (int)(!toggle[0])*2 + (int)(!toggle[1]); + Toggle_speed = (int)(!toggle[2]); + + PS3get(false); + + printf("mode:%d|speed:%d | button:(%d,%d) ", Toggle_mode, Toggle_speed, Button, preButton); + + if (Button) { + switch (Toggle_mode) { + case 0 : + StartDemo(); + break; + case 1 : + printf("Kicker Test \r\n"); + Master.Shot(); + break; + case 2 : + printf("Dribbler Test \r\n"); + Master.Dribble(0.7); + break; + case 3 : + MotorTest(); + break; } - - // モーター出力値の計算 -#ifdef RobotNumberIsOne - // 機体ナンバーが 1 - Vx = ((st[0]-128.0)/128.0) * VxLimit; - Vy = (128.0-st[1])/128.0 * VyLimit; - Vr = -(st[2]-128.0)/128.0 * VrLimit * 0.5; -#else - // 機体ナンバーが 2 - Vx = ((st[0]-128.0)/128.0) * VxLimit; - Vy = (128.0-st[1])/128.0 * VyLimit; - Vr = -(st[2]-128.0)/128.0 * VrLimit * 0.5; -#endif - if (!flag_Rotate) { - Vr = Rotate_Power; - } - - V = hypot(Vx, Vy); - theta = atan2(Vy, Vx); -#ifdef RobotNumberIsOne - omni.computeCircular(V, theta - angleLimit/180.0*PI, Vr); -#else - omni.computeCircular(V, theta - angleLimit/180.0*PI, -Vr); -#endif - - for(int i=0; i < 4; i++) { - if (0) { - if (b[i+4]) { - pwm[i] = 0.5; - // printf("%d ", i); - } else if (b[i+4+4]) { - pwm[i] = -0.5; - } else { - pwm[i] = 0; - } - } else { - pwm[i] = ((double)omni.wheel[i] * Bias_Motor); - } - } - // printf("\r\n"); - - // フラグ処理 - if (b[9]) flag_Dribble = true; - else flag_Dribble = false; - - if (b[8] && (Time_Shot.read_ms() > 1000)) { - flag_Shot = true; - Time_Shot.reset(); - } else { - flag_Shot = false; - } - - // ドリブラー・キッカー - // if (flag_Dribble) esc.setspeed(0.7); - // else esc.setspeed(0.0); - if (flag_Dribble) Master.Dribble(0.7); - else Master.Dribble(0.0); - if (flag_Shot) Master.Shot(); - - // ニュートラル処理 - for (int i=0; i<4; i++) { - if (fabs(pwm[i]) < 0.1) pwm[i] = 0.0; - } - -#ifdef RobotNumberIsOne - Master.SetValueMotor(0, pwm[0]); - Master.SetValueMotor(1, pwm[1]); - Master.SetValueMotor(2, -pwm[2]); - Master.SetValueMotor(3, -pwm[3]); -#else - Master.SetValueMotor(0, pwm[0] * Bias_DefectiveMD); - Master.SetValueMotor(1, pwm[1]); - Master.SetValueMotor(2, pwm[2]); - Master.SetValueMotor(3, pwm[3] * Bias_DefectiveMD); -#endif + } else { + printf("\r\n"); + Master.Dribble(0.0); + for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0); } + preButton = Button; } } @@ -222,4 +145,219 @@ } else { mini.getState(); } +} + +void StartDemo() +{ + bool flag_Dribble=false; + bool flag_Shot=false; + bool flag_Rotate=false; + bool b_[12]; + + double pwm[4]={}; + double DribblePower = 0.5; + double Vx=0; + double Vy=0; + double Vr=0; + double RotatePower=0; + double V=0; + double theta=0; + double rawAngle=0; /// jy901の生データ + double targetAngle=0; /// PIDの目標角(-180 ~ 180) + double limitedAngle=0; /// rawAngleを(-180~180)の範囲で表す + double processAngle=0; /// PIDで補正される値 + + Time_Shot.start(); + jy.calibrateAll(50); + +#ifdef RobotNumberIsOne +// 機体ナンバーが 1 + double VxLimit=0.75; // X軸方向ベクトルの最大値 + double VyLimit=0.75; // Y軸方向ベクトルの最大値 + double VrLimit=0.25; // 回転方向ベクトルの最大値 + double Bias_Motor=0.75; // モーター出力値のバイアス + omni.wheel[0].setRadian(PI * 1.0 / 4.0); + omni.wheel[1].setRadian(PI * 7.0 / 4.0); + omni.wheel[2].setRadian(PI * 5.0 / 4.0); + omni.wheel[3].setRadian(PI * 3.0 / 4.0); +#else +// 機体ナンバーが 2 + double VxLimit=0.8; // X軸方向ベクトルの最大値 + double VyLimit=0.8; // Y軸方向ベクトルの最大値 + double VrLimit=0.3; // 回転方向ベクトルの最大値 + double Bias_Motor=0.8; // モーター出力値のバイアス + double Bias_DefectiveMD = 0.6; // ダメダメMD補正用バイアス + omni.wheel[0].setRadian(PI * 5.0 / 4.0); + omni.wheel[1].setRadian(PI * 7.0 / 4.0); + omni.wheel[2].setRadian(PI * 1.0 / 4.0); + omni.wheel[3].setRadian(PI * 3.0 / 4.0); +#endif + + Rotate.setInputLimits(-360, 360); + Rotate.setOutputLimits(-1*VrLimit,VrLimit); + Rotate.setBias(0); + Rotate.setMode(1); + Rotate.setSetPoint(targetAngle); + +// 以下ループ + while (true) { + PS3get(true); + Button = rawButton; + +// UIボードの処理 + Toggle_mode = (int)(!toggle[0])*2 + (int)(!toggle[1]); + Toggle_speed = (int)(!toggle[2]); + if (!Button && preButton) { + if (!flag_Passfunc) { + flag_Passfunc = true; + } else { + Toggle_mode = 4; + } + } + if (Toggle_mode!=0) { + for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0); + Master.Dribble(0.0); + Time_MotorTest.reset(); + Time_MotorTest.stop(); + flag_Passfunc = false; + return; + } + +// 以下ペアリング中のループ + if (mini.getState()) { + +// ジャイロ + rawAngle = jy.getZaxisAngle(); + if (rawAngle > 180.0) rawAngle -= 360; + if (rawAngle < -180.0) rawAngle += 360; + limitedAngle = rawAngle; + +// 右スティックで回転 + if (fabs(st[2]-128.0) > 30) { + targetAngle = limitedAngle; + flag_Rotate = true; + } else { + flag_Rotate = false; + } + + if ((st[3] > 220) && b[10] && !b_[10]) { + targetAngle = fmod((targetAngle + 180), 360); + } + +// 零点出し + if (b[1]) { + jy.yawcalibrate(); + Rotate.setSetPoint(0); + targetAngle = 0; + } + +// PIDの調整 + processAngle = limitedAngle - targetAngle; + if (processAngle > 180.0) processAngle = processAngle - 360.0; + if (processAngle < -180.0) processAngle = processAngle + 360.0; + Rotate.setProcessValue(processAngle); + RotatePower = Rotate.compute(); + +// モーター出力値の計算 +#ifdef RobotNumberIsOne +// 機体ナンバーが 1 + Vx = ((st[0]-128.0)/128.0) * VxLimit; + Vy = (128.0-st[1])/128.0 * VyLimit; + Vr = -(st[2]-128.0)/128.0 * VrLimit; +#else +// 機体ナンバーが 2 + Vx = ((st[0]-128.0)/128.0) * VxLimit; + Vy = (128.0-st[1])/128.0 * VyLimit; + Vr = -(st[2]-128.0)/128.0 * VrLimit; +#endif + + if (!flag_Rotate) { + Vr = RotatePower; + } + + V = hypot(Vx, Vy); + if (!Toggle_speed) V *= 0.5; + theta = atan2(Vy, Vx); +#ifdef RobotNumberIsOne + omni.computeCircular(V, theta - limitedAngle/180.0*PI, Vr); +#else + omni.computeCircular(V, theta - limitedAngle/180.0*PI, -Vr); +#endif + for(int i=0; i < 4; i++) { + pwm[i] = ((double)omni.wheel[i] * Bias_Motor); + } + +// フラグ処理 + if (b[9]) flag_Dribble = true; + else flag_Dribble = false; + + if (b[8] && (Time_Shot.read_ms() > 1000)) { + flag_Shot = true; + Time_Shot.reset(); + } else { + flag_Shot = false; + } + +// ドリブラー・キッカー + if (flag_Dribble) Master.Dribble(0.7); + else Master.Dribble(0.0); + if (flag_Shot) Master.Shot(); + +// ニュートラル処理 + for (int i=0; i<4; i++) { + if (fabs(pwm[i]) < 0.05) pwm[i] = 0.0; + } + +#ifdef RobotNumberIsOne + Master.SetValueMotor(0, pwm[0]); + Master.SetValueMotor(1, pwm[1]); + Master.SetValueMotor(2, -pwm[2]); + Master.SetValueMotor(3, -pwm[3]); +#else + Master.SetValueMotor(0, pwm[0] * Bias_DefectiveMD); + Master.SetValueMotor(1, pwm[1]); + Master.SetValueMotor(2, pwm[2]); + Master.SetValueMotor(3, pwm[3] * Bias_DefectiveMD); +#endif + + printf("(Vx,Vy, Vr):(%3d,%3d,%3d)|theta:%d | angle:%d | speed:%d | " + , (int)(Vx*100), (int)(Vy*100), (int)(Vr*100), (int)(theta), (int)limitedAngle, Toggle_speed); + } + +// 1ループ前のボタンの値 + preButton = Button; + for (int i=0; i<12; i++) b_[i] = b[i]; + } +} + +void MotorTest() +{ + Time_MotorTest.start(); + uint8_t Num_MotorTest = 0; + double pwm=0.0; + + while(1) { + Button = rawButton; + pwm = sin(Time_MotorTest.read() * PI); + Master.SetValueMotor(Num_MotorTest, pwm); + if (Time_MotorTest.read() > 2.0) { + Master.SetValueMotor(Num_MotorTest, 0.0); + Num_MotorTest = (Num_MotorTest+1) % 4; + Time_MotorTest.reset(); + } + if (!Button && preButton) { + if (!flag_Passfunc) { + flag_Passfunc = true; + } else { + for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0); + Time_MotorTest.reset(); + Time_MotorTest.stop(); + flag_Passfunc = false; + return; + } + } + printf("Motor Test | motor%d:%3d | button:(%d,%d) | \r\n" + , Num_MotorTest, (int)((pwm)*100), (int)Button, preButton); + preButton = Button; + } } \ No newline at end of file