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
diff -r 7da4a8e74955 -r bff63180d3e6 main.cpp --- 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 +// 機体ナンバーが 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) { - - 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 + // 機体ナンバーが 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 { + 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; - } - 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