Ryu Anzawa
doxygen test
Diff: main.cpp
- Revision:
- 3:d3e52f43b427
- Parent:
- 2:98b9c6e44ae4
- Child:
- 4:bbc6c27561e6
--- 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 １：速め ０：遅め
///
/// @details 中央トグル２つでモード切替
-/// @note 中央トグルでのモード切替;
-/// 左０右０：スタート
-/// 左１右０：キッカーテスト
-/// 左０右１：ドリブラーテスト
-/// 左１右１：オムニテスト
+/// @note Refer
+/// - 中央トグルでのモード切替;
+/// - 左０右０：スタート
+/// - 左１右０：キッカーテスト
+/// - 左０右１：ドリブラーテスト
+/// - 左１右１：オムニテスト
+/// - ボタンを押すかトグルスイッチを変えるとワイヤレスモード中止
///
/// @details スタート後にPSボタンを押してペアリング 以下操作方法
-/// @note 左スティックで移動(絶対角)
-/// 右スティックX軸で回転
-/// 〇でドリブル、△でシュート
+/// @note Refer
+/// - 左スティックで移動(絶対角)
+/// - 右スティックX軸で回転
+/// - 〇でドリブル、△でシュート
///
/// @attention １番機は起動後に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
-// 機体ナンバーが １
- double VxLimit=0.75; // Ｘ軸方向ベクトルの最大値
- double VyLimit=0.75; // Ｙ軸方向ベクトルの最大値
- 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
-// 機体ナンバーが ２
- double VxLimit=0.8; // Ｘ軸方向ベクトルの最大値
- double VyLimit=0.8; // Ｙ軸方向ベクトルの最大値
- double VrLimit=0.6; // 回転方向ベクトルの最大値
- double Bias_Motor=0.8; // モーター出力値のバイアス
- double Bias_DefectiveMD = 0.6; // ダメダメＭＤ補正用バイアス
- 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
- // 機体ナンバーが １
- 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
- // 機体ナンバーが ２
- 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
+// 機体ナンバーが １
+ double VxLimit=0.75; // Ｘ軸方向ベクトルの最大値
+ double VyLimit=0.75; // Ｙ軸方向ベクトルの最大値
+ 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
+// 機体ナンバーが ２
+ double VxLimit=0.8; // Ｘ軸方向ベクトルの最大値
+ double VyLimit=0.8; // Ｙ軸方向ベクトルの最大値
+ double VrLimit=0.3; // 回転方向ベクトルの最大値
+ double Bias_Motor=0.8; // モーター出力値のバイアス
+ double Bias_DefectiveMD = 0.6; // ダメダメＭＤ補正用バイアス
+ 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
+// 機体ナンバーが １
+ 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
+// 機体ナンバーが ２
+ 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);
+ }
+
+// １ループ前のボタンの値
+ 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