Quadrifoglio
Communicate with PS3 controller
Dependencies: omni_wheel PID jy901 solenoid Master kohiMD lpf SerialArduino RCJESC
Diff: main.cpp
- Revision:
- 3:bff63180d3e6
- Parent:
- 2:7da4a8e74955
- Child:
- 4:3471cb3eaea4
--- a/main.cpp Wed Jun 15 09:33:38 2022 +0000
+++ b/main.cpp Sat Jun 18 08:43:02 2022 +0000
@@ -5,20 +5,20 @@
//--------操作方法--------
-#include "main.h"
#include "Master.h"
+#include "jy901.h"
+#include "PID.h"
#include "SerialArduino.h"
-//#include "UI.h"
DigitalOut led(LED1);
Master Master;// 出力等の基本クラス
-//Thread DisplayThread;
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);
-//void Display(void);
uint8_t h1,h2;
bool b[12]= {};
@@ -26,88 +26,151 @@
uint8_t tr[2]= {};
int main()
-{
+{
uint8_t mode=0;
bool EdgeToggle[3]={}, pretoggle[3]={};
- bool flag_Dribble=false, flag_Shot=false;
+ 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;
- double VxLimit=0.75, VyLimit=0.75, VrLimit=0.2;
+ 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);
-#if RobotNumberIsOne
+#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) {
-
- PS3get(true);
-
-// モーター出力値の計算
- if (RobotNumberIsOne) {
+ 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;
- } else {
+ 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;
- }
- omni.computeXY(Vx, Vy, Vr);
-
- for(int i=0; i < 4; i++) {
- if (0) {
- if (b[i+4]) {
- pwm[i] = 0.5;
- } else if (b[i+4+4]) {
- pwm[i] = -0.5;
+ 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] = 0;
+ 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 {
- pwm[i] = ((double)(omni.wheel[i])*0.75);
+ 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
}
- if (b[2]) flag_Dribble = true;
- else flag_Dribble = false;
-
- if (b[3] && (Time_Shot.read_ms() > 1000)) {
- flag_Shot = true;
- Time_Shot.reset();
- } else {
- flag_Shot = false;
- }
-
- if (flag_Dribble) Master.Dribble(0.5);
- 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;
- }
-
-#if RobotNumberIsOne
- Master.SetValueMotor(0, pwm[0]);
- Master.SetValueMotor(1, pwm[1]);
- Master.SetValueMotor(2, -pwm[2]);
- Master.SetValueMotor(3, -pwm[3]);
-#else
- for (int i=0; i<4; i++) {
- Master.SetValueMotor(i, pwm[i]);
- }
-#endif
}
}
@@ -124,40 +187,16 @@
for(int i=0; i<4; i++) {
st[i] = mini.getStick(i);
}
- 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]);
-// printf("%3d %3d ", (int)(st[0]-128.0), (int)(128-st[1]));
- if(mini.getState()) printf("ok");
- else printf("bad");
- printf("\r\n");
-}
-
-#if 0
-void Display()
-{
- char lcdname1[4] = {}, lcdname2[4] = {};
- int lcdvalue[2] = {};
- double temp[2] = {};
- while(1){
- switch (ui.displayst) {
- case 0:
- strcpy(lcdname1, "mt0");
- strcpy(lcdname2, "mt1");
- lcdvalue[0] = (int)((double)(omni.wheel[0])*100);
- lcdvalue[1] = (int)((double)(omni.wheel[1])*100);
- break;
- case 1:
- strcpy(lcdname1, "mt2");
- strcpy(lcdname2, "mt3");
- lcdvalue[0] = (int)((double)(omni.wheel[2])*100);
- lcdvalue[1] = (int)((double)(omni.wheel[3])*100);
- break;
- }
- ui.change(lcdname1, lcdname2, lcdvalue);
-// thread_sleep_for(3);
+ 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();
}
-}
-#endif
\ No newline at end of file
+}
\ No newline at end of file