Ryu Anzawa
doxygen test
Diff: main.cpp
- Revision:
- 0:6a6e78ee4e08
- Child:
- 1:e6a7e0b92032
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Jun 19 06:45:51 2022 +0000
@@ -0,0 +1,206 @@
+/// @file main.cpp
+/// @brief ロボカップラジコンのプログラム
+///
+/// 右端トグルは速度調整、中央２つでモード切り替えを行う
+///
+///
+///
+
+//--------操作方法--------
+
+
+#include "Master.h"
+#include "jy901.h"
+#include "PID.h"
+#include "SerialArduino.h"
+
+DigitalOut led(LED1);
+Master Master;// 出力等の基本クラス
+OmniWheel omni(4);
+SerialArduino mini(A0, A1, 115200);// Arduinoを用いてPS3コントローラと通信するクラス
+JY901 jy(jysda, jyscl);
+PID Rotate(6.0, 50.0, 0.0005, 0.01);
+Timer Time_Shot;
+
+void PS3get(bool flag_PS3print);
+
+uint8_t h1,h2;
+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);
+ }
+
+ // モーター出力値の計算
+#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
+ }
+ }
+}
+
+void PS3get(bool flag_PS3print)
+{
+ h1 = mini.getHedder1();
+ h2 = mini.getHedder2();
+ for(int i=0; i<12; i++) {
+ b[i] = mini.getButton(i);
+ }
+ for(int i=0; i<2; i++) {
+ tr[i] = mini.getTrigger(i);
+ }
+ for(int i=0; i<4; i++) {
+ st[i] = mini.getStick(i);
+ }
+ if (flag_PS3print) {
+ for(int i=0; i<12; i++) printf("%d ",b[i]);
+ printf("|");
+ for(int i=0; i<2; i++) printf("%3d ",tr[i]);
+ printf("|");
+ for(int i=0; i<4; i++) printf("%3d ",st[i]);
+ if(mini.getState()) printf("ok");
+ else printf("bad");
+ printf("\r\n");
+ } else {
+ mini.getState();
+ }
+}
\ No newline at end of file