Communicate with PS3 controller
Dependencies: omni_wheel PID jy901 solenoid Master kohiMD lpf SerialArduino RCJESC
main.cpp
- Committer:
- piroro4560
- Date:
- 2022-06-23
- Revision:
- 7:9b4acb93f94d
- Parent:
- 6:cb4a0fc3f90c
File content as of revision 7:9b4acb93f94d:
/// /// @file main.cpp /// @brief ロボカップラジコンのプログラム /// /// @note トグルは上が1で下が0 /// /// @details 右端トグルで速度切り替え /// @note 1:速め 0:遅め /// /// @details 中央トグル2つでモード切替 /// @note Refer /// - 中央トグルでのモード切替; /// - 左0右0:スタート /// - 左1右0:キッカーテスト /// - 左0右1:ドリブラーテスト /// - 左1右1:オムニテスト /// - ボタンを押すかトグルスイッチを変えるとワイヤレスモード中止 /// /// @details スタート後にPSボタンを押してペアリング 以下操作方法 /// @note Refer /// - 左スティックで移動(絶対角) /// - 右スティックX軸で回転 /// - 〇でドリブル、△でシュート /// /// @attention 1番機は起動後にArduinoのリセットボタンを押す必要あり /// #include "Master.h" #include "jy901.h" #include "PID.h" #include "omni_wheel.h" #include "SerialArduino.h" DigitalOut led(LED1); 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); Timer Time_Shot; Timer Time_MotorTest; #ifdef RobotNumberIsOne PID Rotate(25.0, 100.0, 0.0001, 0.01); #else PID Rotate(15.0, 100.0, 0.0001, 0.01); #endif /// /// @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]= {}; uint8_t st[4]= {}; uint8_t tr[2]= {}; int main() { 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; } } else { printf("\r\n"); Master.Dribble(0.0); for (int i=0; i<4; i++) Master.SetValueMotor(i, 0.0); } preButton = Button; } } 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(); } } 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 // 機体ナンバーが 1 double VxLimit=0.75; // X軸方向ベクトルの最大値 double VyLimit=0.75; // Y軸方向ベクトルの最大値 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 // 機体ナンバーが 2 double VxLimit=0.8; // X軸方向ベクトルの最大値 double VyLimit=0.8; // Y軸方向ベクトルの最大値 double VrLimit=0.3; // 回転方向ベクトルの最大値 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(-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 // 機体ナンバーが 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 // 機体ナンバーが 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; #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); } // 1ループ前のボタンの値 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; } }