kohei matsumoto / Mbed 2 deprecated spring_ps3_3

test

Dependencies:   mbed KondoServoLibrary Encoder

User.cpp@21:0f12ee2322e4, 2020-03-12 (annotated)

Committer:
koheim
Date:
Thu Mar 12 07:32:03 2020 +0000
Revision:
21:0f12ee2322e4
Parent:
20:dd4634500bb3 
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirokimineshita 0:736c76a75def 1 #include "Utils.h"
hirokimineshita 0:736c76a75def 2 #include "USBHost.h"
hirokimineshita 0:736c76a75def 3 #include "hci.h"
hirokimineshita 0:736c76a75def 4 #include "ps3.h"
hirokimineshita 0:736c76a75def 5 #include "User.h"
o2132613 19:510b87211851 6 #include "mbed.h"
o2132613 19:510b87211851 7 #include "math.h"
hirokimineshita 0:736c76a75def 8 #include "mbed.h"
o2132613 19:510b87211851 9 #include"KondoServo.h"
o2132613 19:510b87211851 10 #include "EC.h" //Encoderライブラリをインクルード
o2132613 19:510b87211851 11 #define RESOLUTION 500
hirokimineshita 0:736c76a75def 12
hirokimineshita 0:736c76a75def 13 int RSX,RSY,LSX,LSY,BSU,BSL;
o2132613 19:510b87211851 14 //Digitalout cs(p8);
o2132613 19:510b87211851 15 int move=5,shoot=0,ball=0,box=0;
o2132613 19:510b87211851 16 CAN controller(p30,p29);//CANpin_name
koheim 21:0f12ee2322e4 17
o2132613 19:510b87211851 18 DigitalOut led1(LED1);
o2132613 19:510b87211851 19 DigitalOut led2(LED2);
o2132613 19:510b87211851 20 DigitalOut led3(LED3);
o2132613 19:510b87211851 21 DigitalOut led4(LED4);
o2132613 19:510b87211851 22 char can[1]= {0};
koheim 21:0f12ee2322e4 23
o2132613 19:510b87211851 24 KondoServo servo(p28,p27);//サーボ
o2132613 19:510b87211851 25
o2132613 19:510b87211851 26 PwmOut f(p26);//投F
o2132613 19:510b87211851 27 PwmOut b(p25);//投B
koheim 21:0f12ee2322e4 28
o2132613 19:510b87211851 29 Ticker ticker;
koheim 21:0f12ee2322e4 30
o2132613 19:510b87211851 31 Ec1multi EC(p16,p17,RESOLUTION);//投E
o2132613 19:510b87211851 32 InterruptIn X(p15);
koheim 21:0f12ee2322e4 33
o2132613 19:510b87211851 34 DigitalOut out1(p19);//エアシリンダー（つかむ方）
o2132613 19:510b87211851 35 DigitalOut out2(p20);//エアシリンダー（投擲）
koheim 21:0f12ee2322e4 36 DigitalOut out3(p18);//エアシリンダー（ボックス）
baba2357 11:86d717718dbf 37
maxnagazumi 20:dd4634500bb3 38 int id = 0;
koheim 21:0f12ee2322e4 39 int id2 = 1;
o2132613 19:510b87211851 40 double SERVO2DEG = 270.0 / (11500 - 3500);
o2132613 19:510b87211851 41 double first = (6800 - 3500) * SERVO2DEG;
maxnagazumi 20:dd4634500bb3 42 double grab = (3600 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 43 double pass = (5000 - 3500) * SERVO2DEG;
koheim 21:0f12ee2322e4 44 double grab2 = (3550 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 45
o2132613 19:510b87211851 46 void cal()
o2132613 19:510b87211851 47 {
o2132613 19:510b87211851 48 EC.calOmega();
o2132613 19:510b87211851 49 }
maxnagazumi 20:dd4634500bb3 50
o2132613 19:510b87211851 51 void shot();
o2132613 19:510b87211851 52 void catch_ball();
maxnagazumi 20:dd4634500bb3 53 void show_angle();
o2132613 19:510b87211851 54
o2132613 19:510b87211851 55 int X_count=0;
o2132613 19:510b87211851 56 void Xcount()
o2132613 19:510b87211851 57 {
o2132613 19:510b87211851 58 X_count++;
o2132613 19:510b87211851 59 }
o2132613 19:510b87211851 60
o2132613 17:348c660ea6f6 61 void UserLoopSetting()
o2132613 17:348c660ea6f6 62 {
hirokimineshita 0:736c76a75def 63 }
o2132613 17:348c660ea6f6 64 void UserLoop(char n,const u8* data)
o2132613 17:348c660ea6f6 65 {
hirokimineshita 0:736c76a75def 66 u16 ButtonState;
o2132613 17:348c660ea6f6 67 if(n==0) { //有線Ps3USB.cpp
hirokimineshita 0:736c76a75def 68 RSX = ((ps3report*)data)->RightStickX;
hirokimineshita 0:736c76a75def 69 RSY = ((ps3report*)data)->RightStickY;
hirokimineshita 0:736c76a75def 70 LSX = ((ps3report*)data)->LeftStickX;
hirokimineshita 0:736c76a75def 71 LSY = ((ps3report*)data)->LeftStickY;
hirokimineshita 0:736c76a75def 72 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 73 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
hirokimineshita 0:736c76a75def 74 //ボタンの処理
hirokimineshita 0:736c76a75def 75 ButtonState = ((ps3report*)data)->ButtonState;
o2132613 17:348c660ea6f6 76 } else {//無線TestShell.cpp
hirokimineshita 0:736c76a75def 77 RSX = ((ps3report*)(data + 1))->RightStickX;
hirokimineshita 0:736c76a75def 78 RSY = ((ps3report*)(data + 1))->RightStickY;
hirokimineshita 0:736c76a75def 79 LSX = ((ps3report*)(data + 1))->LeftStickX;
hirokimineshita 0:736c76a75def 80 LSY = ((ps3report*)(data + 1))->LeftStickY;
hirokimineshita 0:736c76a75def 81 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 82 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
hirokimineshita 0:736c76a75def 83 //ボタンの処理
hirokimineshita 0:736c76a75def 84 ButtonState = ((ps3report*)(data + 1))->ButtonState;
hirokimineshita 0:736c76a75def 85 }
hirokimineshita 0:736c76a75def 86 //ここより下にプログラムを書く
o2132613 19:510b87211851 87 controller.frequency(1000000);
o2132613 19:510b87211851 88 led1=0;
o2132613 19:510b87211851 89 led2=0;
o2132613 19:510b87211851 90 led3=0;
o2132613 19:510b87211851 91 led4=0;
o2132613 19:510b87211851 92
o2132613 19:510b87211851 93 //move
o2132613 19:510b87211851 94 if((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //UPL1
o2132613 19:510b87211851 95 move = 9;
o2132613 19:510b87211851 96 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //DownR1
o2132613 19:510b87211851 97 move = 3;
o2132613 19:510b87211851 98 } else if ((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //UPR1
o2132613 19:510b87211851 99 move = 7;
o2132613 19:510b87211851 100 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //DownL1
o2132613 19:510b87211851 101 move = 1;
o2132613 19:510b87211851 102 } else if((ButtonState >> BUTTONUP)&1 == 1) { //up
o2132613 19:510b87211851 103 move = 8;
o2132613 19:510b87211851 104 led1 = 1;
o2132613 19:510b87211851 105 } else if((ButtonState >> BUTTONDOWN)&1 == 1) { //down
o2132613 19:510b87211851 106 move = 2;
o2132613 19:510b87211851 107 led2 =1;
o2132613 19:510b87211851 108 } else if((ButtonState >> BUTTONRIGHT)&1 == 1) { //right
o2132613 19:510b87211851 109 move = 6;
o2132613 19:510b87211851 110 led3 = 1;
o2132613 19:510b87211851 111 } else if((ButtonState >> BUTTONLEFT)&1 == 1) { //left
o2132613 19:510b87211851 112 move = 4;
o2132613 19:510b87211851 113 led4 = 1;
o2132613 19:510b87211851 114 } else if((ButtonState >> BUTTONL1)&1 == 1) { //L1
o2132613 19:510b87211851 115 move = 10;
o2132613 19:510b87211851 116 } else if((ButtonState >> BUTTONR1)&1 == 1) { //R1
o2132613 19:510b87211851 117 move = 11;
o2132613 19:510b87211851 118 } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
o2132613 19:510b87211851 119 move = 1;
o2132613 19:510b87211851 120 } else {
o2132613 19:510b87211851 121 move = 5;
o2132613 19:510b87211851 122 }
o2132613 19:510b87211851 123 //move
o2132613 19:510b87211851 124 //shoot
o2132613 19:510b87211851 125 if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
o2132613 19:510b87211851 126 shoot = 1;
o2132613 19:510b87211851 127 led1 = 1;
o2132613 19:510b87211851 128 } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
maxnagazumi 20:dd4634500bb3 129 shoot = 4;
o2132613 19:510b87211851 130 led2 = 1;
o2132613 19:510b87211851 131 } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
o2132613 19:510b87211851 132 shoot = 3;
o2132613 19:510b87211851 133 led3 = 1;
o2132613 19:510b87211851 134 } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
maxnagazumi 20:dd4634500bb3 135 shoot = 2;
maxnagazumi 20:dd4634500bb3 136 led4 = 1;
maxnagazumi 20:dd4634500bb3 137 } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
maxnagazumi 20:dd4634500bb3 138 shoot = 5;
o2132613 19:510b87211851 139 led4 = 1;
o2132613 17:348c660ea6f6 140 }
maxnagazumi 20:dd4634500bb3 141 show_angle();
o2132613 19:510b87211851 142 //CAN通信用プログラム
o2132613 19:510b87211851 143 if(shoot==1)shot();
o2132613 19:510b87211851 144 if(shoot>=2)catch_ball();
o2132613 19:510b87211851 145 can[0] = move;
o2132613 19:510b87211851 146 controller.write(CANMessage(1,can,1));
o2132613 19:510b87211851 147 printf("%d\r\n",can[0]);
o2132613 19:510b87211851 148 }
o2132613 19:510b87211851 149
maxnagazumi 20:dd4634500bb3 150 void show_angle()
maxnagazumi 20:dd4634500bb3 151 {
maxnagazumi 20:dd4634500bb3 152 X.rise(&Xcount);
maxnagazumi 20:dd4634500bb3 153 f.period_us(50);
maxnagazumi 20:dd4634500bb3 154 b.period_us(50);
maxnagazumi 20:dd4634500bb3 155 double a=0,r=0.4,v=0;
maxnagazumi 20:dd4634500bb3 156 int i=0,count;//rで半径指定 a*r=v
maxnagazumi 20:dd4634500bb3 157 //ticker.attach(&cal,0.05);
maxnagazumi 20:dd4634500bb3 158 if(X_count ==1) {
maxnagazumi 20:dd4634500bb3 159 printf("set ok");
maxnagazumi 20:dd4634500bb3 160 EC.reset();
maxnagazumi 20:dd4634500bb3 161 }
maxnagazumi 20:dd4634500bb3 162 //角度リセット
maxnagazumi 20:dd4634500bb3 163 a=EC.getOmega();
maxnagazumi 20:dd4634500bb3 164 count=EC.getCount();
maxnagazumi 20:dd4634500bb3 165 i =count%500;
maxnagazumi 20:dd4634500bb3 166 printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count);
maxnagazumi 20:dd4634500bb3 167 f=0;
maxnagazumi 20:dd4634500bb3 168 b=0;
maxnagazumi 20:dd4634500bb3 169 led1=0;
maxnagazumi 20:dd4634500bb3 170 led2=1;
maxnagazumi 20:dd4634500bb3 171 led3=1;
maxnagazumi 20:dd4634500bb3 172 led4=0;
maxnagazumi 20:dd4634500bb3 173
maxnagazumi 20:dd4634500bb3 174 }
o2132613 19:510b87211851 175 void shot()
o2132613 19:510b87211851 176 {
o2132613 19:510b87211851 177 X.rise(&Xcount);
o2132613 19:510b87211851 178 f.period_us(50);
o2132613 19:510b87211851 179 b.period_us(50);
o2132613 19:510b87211851 180 double a=0,r=0.4,v=0;
o2132613 19:510b87211851 181 int i=0,count;//rで半径指定 a*r=v
o2132613 19:510b87211851 182 ticker.attach(&cal,0.05);
maxnagazumi 20:dd4634500bb3 183 while(1) {
maxnagazumi 20:dd4634500bb3 184 printf("set");
maxnagazumi 20:dd4634500bb3 185 if(X_count ==1) {
maxnagazumi 20:dd4634500bb3 186 EC.reset();
maxnagazumi 20:dd4634500bb3 187 X_count =0;
maxnagazumi 20:dd4634500bb3 188 }
maxnagazumi 20:dd4634500bb3 189 break;
o2132613 17:348c660ea6f6 190 }
o2132613 19:510b87211851 191 //角度リセット
o2132613 19:510b87211851 192 while(1) {
o2132613 19:510b87211851 193 printf("%.3f %.3f %d %d\r\n",a,v,X_count,i);
o2132613 19:510b87211851 194 a=EC.getOmega();
o2132613 19:510b87211851 195 count=EC.getCount();
o2132613 19:510b87211851 196 i =count%500;
o2132613 19:510b87211851 197 v=a*r;
o2132613 19:510b87211851 198 f=0.45;//速度一定
o2132613 19:510b87211851 199 b=0.0;
o2132613 19:510b87211851 200 led1=1;
o2132613 19:510b87211851 201 led2=0;
o2132613 19:510b87211851 202 led3=0;
o2132613 19:510b87211851 203 led4=1;
o2132613 19:510b87211851 204 if(X_count>3) {
o2132613 19:510b87211851 205 if(i < -380 && i > -390) {//離す角度を決める
o2132613 19:510b87211851 206 out2=1; //ボールを離す
o2132613 19:510b87211851 207 printf("shot\r\n");
o2132613 19:510b87211851 208 led1=1;
o2132613 19:510b87211851 209 led2=1;
o2132613 19:510b87211851 210 led3=1;
o2132613 19:510b87211851 211 led4=1;
o2132613 19:510b87211851 212 break;
o2132613 19:510b87211851 213 }
o2132613 19:510b87211851 214 }
o2132613 17:348c660ea6f6 215 }
o2132613 19:510b87211851 216 printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i);
o2132613 19:510b87211851 217 X.rise(&Xcount);
o2132613 19:510b87211851 218 a=EC.getOmega();
o2132613 19:510b87211851 219 count=EC.getCount();
o2132613 19:510b87211851 220 i =count%500;
o2132613 19:510b87211851 221 f=0;
o2132613 19:510b87211851 222 b=0;
o2132613 19:510b87211851 223 led1=0;
o2132613 19:510b87211851 224 led2=1;
o2132613 19:510b87211851 225 led3=1;
o2132613 19:510b87211851 226 led4=0;
o2132613 19:510b87211851 227
o2132613 19:510b87211851 228 }
o2132613 19:510b87211851 229
o2132613 19:510b87211851 230 void catch_ball()
o2132613 19:510b87211851 231 {
o2132613 19:510b87211851 232 switch (shoot) {
o2132613 19:510b87211851 233 case 2:
maxnagazumi 20:dd4634500bb3 234 if(ball ==0) {
maxnagazumi 20:dd4634500bb3 235 servo.set_degree(id, first);
maxnagazumi 20:dd4634500bb3 236 wait(0.5);
maxnagazumi 20:dd4634500bb3 237 shoot =0;
maxnagazumi 20:dd4634500bb3 238 ball = 1;
maxnagazumi 20:dd4634500bb3 239 } else if(ball ==1) {
maxnagazumi 20:dd4634500bb3 240 servo.set_degree(id, grab);
maxnagazumi 20:dd4634500bb3 241 wait(0.5);
maxnagazumi 20:dd4634500bb3 242 shoot = 0;
maxnagazumi 20:dd4634500bb3 243 ball =0;
maxnagazumi 20:dd4634500bb3 244 }
maxnagazumi 20:dd4634500bb3 245 break;
o2132613 19:510b87211851 246 case 3:
koheim 21:0f12ee2322e4 247 servo.set_degree(id2, grab2);
maxnagazumi 20:dd4634500bb3 248 break;
o2132613 19:510b87211851 249 case 4:
maxnagazumi 20:dd4634500bb3 250 if (out1==0) {
maxnagazumi 20:dd4634500bb3 251 out1=1;
maxnagazumi 20:dd4634500bb3 252 shoot=0;
maxnagazumi 20:dd4634500bb3 253 } else if (out1 == 1) {
maxnagazumi 20:dd4634500bb3 254 out1=0;
maxnagazumi 20:dd4634500bb3 255 shoot=0;
maxnagazumi 20:dd4634500bb3 256 }
maxnagazumi 20:dd4634500bb3 257 break;
maxnagazumi 20:dd4634500bb3 258 case 5:
koheim 21:0f12ee2322e4 259 if (out3==0) {
koheim 21:0f12ee2322e4 260 out3=1;
koheim 21:0f12ee2322e4 261 } else if (out3 == 1) {
koheim 21:0f12ee2322e4 262 out3=0;
maxnagazumi 20:dd4634500bb3 263 }
o2132613 19:510b87211851 264 }
o2132613 19:510b87211851 265 }
o2132613 19:510b87211851 266
o2132613 19:510b87211851 267 void catch_box()
o2132613 19:510b87211851 268 {
o2132613 19:510b87211851 269
hirokimineshita 0:736c76a75def 270 }