Communicate with PS3 controller
Dependencies: omni_wheel PID jy901 solenoid Master kohiMD lpf SerialArduino RCJESC
main.cpp@2:7da4a8e74955, 2022-06-15 (annotated)
- Committer:
- piroro4560
- Date:
- Wed Jun 15 09:33:38 2022 +0000
- Revision:
- 2:7da4a8e74955
- Parent:
- 1:692623e55ff7
- Child:
- 3:bff63180d3e6
Stick
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piroro4560 | 0:00ce070b2f06 | 1 | /** 300_PS3com |
piroro4560 | 0:00ce070b2f06 | 2 | * PS3と通信して表示、ボタンに応じてモーターを動かす |
piroro4560 | 0:00ce070b2f06 | 3 | */ |
piroro4560 | 0:00ce070b2f06 | 4 | |
piroro4560 | 0:00ce070b2f06 | 5 | //--------操作方法-------- |
piroro4560 | 0:00ce070b2f06 | 6 | |
piroro4560 | 0:00ce070b2f06 | 7 | |
piroro4560 | 0:00ce070b2f06 | 8 | #include "main.h" |
piroro4560 | 0:00ce070b2f06 | 9 | #include "Master.h" |
piroro4560 | 0:00ce070b2f06 | 10 | #include "SerialArduino.h" |
piroro4560 | 2:7da4a8e74955 | 11 | //#include "UI.h" |
piroro4560 | 0:00ce070b2f06 | 12 | |
piroro4560 | 0:00ce070b2f06 | 13 | DigitalOut led(LED1); |
piroro4560 | 0:00ce070b2f06 | 14 | Master Master;// 出力等の基本クラス |
piroro4560 | 2:7da4a8e74955 | 15 | //Thread DisplayThread; |
piroro4560 | 2:7da4a8e74955 | 16 | OmniWheel omni(4); |
piroro4560 | 0:00ce070b2f06 | 17 | SerialArduino mini(A0, A1, 115200);// Arduinoを用いてPS3コントローラと通信するクラス |
piroro4560 | 2:7da4a8e74955 | 18 | Timer Time_Shot; |
piroro4560 | 0:00ce070b2f06 | 19 | |
piroro4560 | 2:7da4a8e74955 | 20 | void PS3get(bool flag_PS3print); |
piroro4560 | 2:7da4a8e74955 | 21 | //void Display(void); |
piroro4560 | 0:00ce070b2f06 | 22 | |
piroro4560 | 1:692623e55ff7 | 23 | uint8_t h1,h2; |
piroro4560 | 1:692623e55ff7 | 24 | bool b[12]= {}; |
piroro4560 | 1:692623e55ff7 | 25 | uint8_t st[4]= {}; |
piroro4560 | 1:692623e55ff7 | 26 | uint8_t tr[2]= {}; |
piroro4560 | 1:692623e55ff7 | 27 | |
piroro4560 | 0:00ce070b2f06 | 28 | int main() |
piroro4560 | 2:7da4a8e74955 | 29 | { |
piroro4560 | 0:00ce070b2f06 | 30 | uint8_t mode=0; |
piroro4560 | 2:7da4a8e74955 | 31 | bool EdgeToggle[3]={}, pretoggle[3]={}; |
piroro4560 | 2:7da4a8e74955 | 32 | bool flag_Dribble=false, flag_Shot=false; |
piroro4560 | 2:7da4a8e74955 | 33 | Time_Shot.start(); |
piroro4560 | 0:00ce070b2f06 | 34 | |
piroro4560 | 2:7da4a8e74955 | 35 | double pwm[4]={}, DribblePower = 0.5; |
piroro4560 | 2:7da4a8e74955 | 36 | double Vx=0, Vy=0, Vr=0; |
piroro4560 | 2:7da4a8e74955 | 37 | double VxLimit=0.75, VyLimit=0.75, VrLimit=0.2; |
piroro4560 | 0:00ce070b2f06 | 38 | |
piroro4560 | 1:692623e55ff7 | 39 | Master.SetPS3Address(b, tr, st); |
piroro4560 | 2:7da4a8e74955 | 40 | |
piroro4560 | 2:7da4a8e74955 | 41 | // DisplayThread.start(Display); |
piroro4560 | 2:7da4a8e74955 | 42 | #if RobotNumberIsOne |
piroro4560 | 2:7da4a8e74955 | 43 | omni.wheel[0].setRadian(PI * 1.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 44 | omni.wheel[1].setRadian(PI * 7.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 45 | omni.wheel[2].setRadian(PI * 5.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 46 | omni.wheel[3].setRadian(PI * 3.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 47 | #else |
piroro4560 | 2:7da4a8e74955 | 48 | omni.wheel[0].setRadian(PI * 5.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 49 | omni.wheel[1].setRadian(PI * 7.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 50 | omni.wheel[2].setRadian(PI * 1.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 51 | omni.wheel[3].setRadian(PI * 3.0 / 4.0); |
piroro4560 | 2:7da4a8e74955 | 52 | #endif |
piroro4560 | 1:692623e55ff7 | 53 | // 以下ループ |
piroro4560 | 0:00ce070b2f06 | 54 | while (true) { |
piroro4560 | 2:7da4a8e74955 | 55 | |
piroro4560 | 2:7da4a8e74955 | 56 | PS3get(true); |
piroro4560 | 0:00ce070b2f06 | 57 | |
piroro4560 | 0:00ce070b2f06 | 58 | // モーター出力値の計算 |
piroro4560 | 2:7da4a8e74955 | 59 | if (RobotNumberIsOne) { |
piroro4560 | 2:7da4a8e74955 | 60 | Vx = ((st[0]-128.0)/128.0) * VxLimit; |
piroro4560 | 2:7da4a8e74955 | 61 | Vy = (128.0-st[1])/128.0 * VyLimit; |
piroro4560 | 2:7da4a8e74955 | 62 | Vr = -(st[2]-128.0)/128.0 * VrLimit; |
piroro4560 | 2:7da4a8e74955 | 63 | } else { |
piroro4560 | 2:7da4a8e74955 | 64 | Vx = ((st[0]-128.0)/128.0) * VxLimit; |
piroro4560 | 2:7da4a8e74955 | 65 | Vy = (128.0-st[1])/128.0 * VyLimit; |
piroro4560 | 2:7da4a8e74955 | 66 | Vr = (st[2]-128.0)/128.0 * VrLimit; |
piroro4560 | 2:7da4a8e74955 | 67 | } |
piroro4560 | 2:7da4a8e74955 | 68 | omni.computeXY(Vx, Vy, Vr); |
piroro4560 | 2:7da4a8e74955 | 69 | |
piroro4560 | 0:00ce070b2f06 | 70 | for(int i=0; i < 4; i++) { |
piroro4560 | 2:7da4a8e74955 | 71 | if (0) { |
piroro4560 | 2:7da4a8e74955 | 72 | if (b[i+4]) { |
piroro4560 | 2:7da4a8e74955 | 73 | pwm[i] = 0.5; |
piroro4560 | 2:7da4a8e74955 | 74 | } else if (b[i+4+4]) { |
piroro4560 | 2:7da4a8e74955 | 75 | pwm[i] = -0.5; |
piroro4560 | 2:7da4a8e74955 | 76 | } else { |
piroro4560 | 2:7da4a8e74955 | 77 | pwm[i] = 0; |
piroro4560 | 2:7da4a8e74955 | 78 | } |
piroro4560 | 0:00ce070b2f06 | 79 | } else { |
piroro4560 | 2:7da4a8e74955 | 80 | pwm[i] = ((double)(omni.wheel[i])*0.75); |
piroro4560 | 0:00ce070b2f06 | 81 | } |
piroro4560 | 0:00ce070b2f06 | 82 | } |
piroro4560 | 2:7da4a8e74955 | 83 | if (b[2]) flag_Dribble = true; |
piroro4560 | 2:7da4a8e74955 | 84 | else flag_Dribble = false; |
piroro4560 | 2:7da4a8e74955 | 85 | |
piroro4560 | 2:7da4a8e74955 | 86 | if (b[3] && (Time_Shot.read_ms() > 1000)) { |
piroro4560 | 2:7da4a8e74955 | 87 | flag_Shot = true; |
piroro4560 | 2:7da4a8e74955 | 88 | Time_Shot.reset(); |
piroro4560 | 2:7da4a8e74955 | 89 | } else { |
piroro4560 | 2:7da4a8e74955 | 90 | flag_Shot = false; |
piroro4560 | 2:7da4a8e74955 | 91 | } |
piroro4560 | 2:7da4a8e74955 | 92 | |
piroro4560 | 2:7da4a8e74955 | 93 | if (flag_Dribble) Master.Dribble(0.5); |
piroro4560 | 2:7da4a8e74955 | 94 | else Master.Dribble(0.0); |
piroro4560 | 2:7da4a8e74955 | 95 | if (flag_Shot) Master.Shot(); |
piroro4560 | 2:7da4a8e74955 | 96 | |
piroro4560 | 2:7da4a8e74955 | 97 | for (int i=0; i<4; i++) { |
piroro4560 | 2:7da4a8e74955 | 98 | if (fabs(pwm[i]) < 0.1) pwm[i] = 0.0; |
piroro4560 | 2:7da4a8e74955 | 99 | } |
piroro4560 | 0:00ce070b2f06 | 100 | |
piroro4560 | 2:7da4a8e74955 | 101 | #if RobotNumberIsOne |
piroro4560 | 2:7da4a8e74955 | 102 | Master.SetValueMotor(0, pwm[0]); |
piroro4560 | 2:7da4a8e74955 | 103 | Master.SetValueMotor(1, pwm[1]); |
piroro4560 | 2:7da4a8e74955 | 104 | Master.SetValueMotor(2, -pwm[2]); |
piroro4560 | 2:7da4a8e74955 | 105 | Master.SetValueMotor(3, -pwm[3]); |
piroro4560 | 2:7da4a8e74955 | 106 | #else |
piroro4560 | 1:692623e55ff7 | 107 | for (int i=0; i<4; i++) { |
piroro4560 | 1:692623e55ff7 | 108 | Master.SetValueMotor(i, pwm[i]); |
piroro4560 | 1:692623e55ff7 | 109 | } |
piroro4560 | 2:7da4a8e74955 | 110 | #endif |
piroro4560 | 0:00ce070b2f06 | 111 | } |
piroro4560 | 1:692623e55ff7 | 112 | } |
piroro4560 | 1:692623e55ff7 | 113 | |
piroro4560 | 2:7da4a8e74955 | 114 | void PS3get(bool flag_PS3print) |
piroro4560 | 1:692623e55ff7 | 115 | { |
piroro4560 | 1:692623e55ff7 | 116 | h1 = mini.getHedder1(); |
piroro4560 | 1:692623e55ff7 | 117 | h2 = mini.getHedder2(); |
piroro4560 | 1:692623e55ff7 | 118 | for(int i=0; i<12; i++) { |
piroro4560 | 1:692623e55ff7 | 119 | b[i] = mini.getButton(i); |
piroro4560 | 1:692623e55ff7 | 120 | } |
piroro4560 | 1:692623e55ff7 | 121 | for(int i=0; i<2; i++) { |
piroro4560 | 1:692623e55ff7 | 122 | tr[i] = mini.getTrigger(i); |
piroro4560 | 1:692623e55ff7 | 123 | } |
piroro4560 | 1:692623e55ff7 | 124 | for(int i=0; i<4; i++) { |
piroro4560 | 1:692623e55ff7 | 125 | st[i] = mini.getStick(i); |
piroro4560 | 1:692623e55ff7 | 126 | } |
piroro4560 | 1:692623e55ff7 | 127 | for(int i=0; i<12; i++) printf("%d ",b[i]); |
piroro4560 | 1:692623e55ff7 | 128 | printf("|"); |
piroro4560 | 1:692623e55ff7 | 129 | for(int i=0; i<2; i++) printf("%3d ",tr[i]); |
piroro4560 | 1:692623e55ff7 | 130 | printf("|"); |
piroro4560 | 1:692623e55ff7 | 131 | for(int i=0; i<4; i++) printf("%3d ",st[i]); |
piroro4560 | 2:7da4a8e74955 | 132 | // printf("%3d %3d ", (int)(st[0]-128.0), (int)(128-st[1])); |
piroro4560 | 1:692623e55ff7 | 133 | if(mini.getState()) printf("ok"); |
piroro4560 | 1:692623e55ff7 | 134 | else printf("bad"); |
piroro4560 | 1:692623e55ff7 | 135 | printf("\r\n"); |
piroro4560 | 1:692623e55ff7 | 136 | } |
piroro4560 | 2:7da4a8e74955 | 137 | |
piroro4560 | 2:7da4a8e74955 | 138 | #if 0 |
piroro4560 | 2:7da4a8e74955 | 139 | void Display() |
piroro4560 | 2:7da4a8e74955 | 140 | { |
piroro4560 | 2:7da4a8e74955 | 141 | char lcdname1[4] = {}, lcdname2[4] = {}; |
piroro4560 | 2:7da4a8e74955 | 142 | int lcdvalue[2] = {}; |
piroro4560 | 2:7da4a8e74955 | 143 | double temp[2] = {}; |
piroro4560 | 2:7da4a8e74955 | 144 | while(1){ |
piroro4560 | 2:7da4a8e74955 | 145 | switch (ui.displayst) { |
piroro4560 | 2:7da4a8e74955 | 146 | case 0: |
piroro4560 | 2:7da4a8e74955 | 147 | strcpy(lcdname1, "mt0"); |
piroro4560 | 2:7da4a8e74955 | 148 | strcpy(lcdname2, "mt1"); |
piroro4560 | 2:7da4a8e74955 | 149 | lcdvalue[0] = (int)((double)(omni.wheel[0])*100); |
piroro4560 | 2:7da4a8e74955 | 150 | lcdvalue[1] = (int)((double)(omni.wheel[1])*100); |
piroro4560 | 2:7da4a8e74955 | 151 | break; |
piroro4560 | 2:7da4a8e74955 | 152 | case 1: |
piroro4560 | 2:7da4a8e74955 | 153 | strcpy(lcdname1, "mt2"); |
piroro4560 | 2:7da4a8e74955 | 154 | strcpy(lcdname2, "mt3"); |
piroro4560 | 2:7da4a8e74955 | 155 | lcdvalue[0] = (int)((double)(omni.wheel[2])*100); |
piroro4560 | 2:7da4a8e74955 | 156 | lcdvalue[1] = (int)((double)(omni.wheel[3])*100); |
piroro4560 | 2:7da4a8e74955 | 157 | break; |
piroro4560 | 2:7da4a8e74955 | 158 | } |
piroro4560 | 2:7da4a8e74955 | 159 | ui.change(lcdname1, lcdname2, lcdvalue); |
piroro4560 | 2:7da4a8e74955 | 160 | // thread_sleep_for(3); |
piroro4560 | 2:7da4a8e74955 | 161 | } |
piroro4560 | 2:7da4a8e74955 | 162 | } |
piroro4560 | 1:692623e55ff7 | 163 | #endif |