Quadrifoglio / Mbed OS 300_PS3com

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?

UserRevisionLine numberNew 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