Communicate with PS3 controller
Dependencies: omni_wheel PID jy901 solenoid Master kohiMD lpf SerialArduino RCJESC
main.cpp
- Committer:
- piroro4560
- Date:
- 2022-06-19
- Revision:
- 4:3471cb3eaea4
- Parent:
- 3:bff63180d3e6
- Child:
- 5:232c0cfdec93
File content as of revision 4:3471cb3eaea4:
/// @file main.cpp /// @brief ロボカップラジコンのプログラム /// /// 右端トグルは速度調整、中央2つでモード切り替えを行う /// /// /// //--------操作方法-------- #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 // 機体ナンバーが 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); } // モーター出力値の計算 #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 } } } 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(); } }