春ロボ　ロケット団 / Mbed 2 deprecated spring_ps3_3

version which is easier to read

Dependencies:   mbed KondoServoLibrary Encoder

User.cpp@23:0c6bc29c8f86, 2020-03-13 (annotated)

Committer:
o2132613
Date:
Fri Mar 13 08:23:05 2020 +0000
Revision:
23:0c6bc29c8f86
Parent:
22:f0e17f3129cc
Child:
24:674b011da45b
correct

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 22:f0e17f3129cc 15 int move=5,shoot=0,ball=0,box=0,ball_count=0,box_count=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 23:0c6bc29c8f86 41 double first_ball = (6800 - 3500) * SERVO2DEG;
o2132613 23:0c6bc29c8f86 42 double first_box = (X - 3500) * SERVO2DEG; //X haven't be decided
o2132613 22:f0e17f3129cc 43 double grab_ball = (3600 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 44 double pass = (5000 - 3500) * SERVO2DEG;
o2132613 22:f0e17f3129cc 45 double grab_box = (3550 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 46
o2132613 19:510b87211851 47 void cal()
o2132613 19:510b87211851 48 {
o2132613 19:510b87211851 49 EC.calOmega();
o2132613 19:510b87211851 50 }
maxnagazumi 20:dd4634500bb3 51
o2132613 19:510b87211851 52 void shot();
o2132613 19:510b87211851 53 void catch_ball();
maxnagazumi 20:dd4634500bb3 54 void show_angle();
o2132613 19:510b87211851 55
o2132613 19:510b87211851 56 int X_count=0;
o2132613 19:510b87211851 57 void Xcount()
o2132613 19:510b87211851 58 {
o2132613 19:510b87211851 59 X_count++;
o2132613 19:510b87211851 60 }
o2132613 19:510b87211851 61
o2132613 17:348c660ea6f6 62 void UserLoopSetting()
o2132613 17:348c660ea6f6 63 {
hirokimineshita 0:736c76a75def 64 }
o2132613 17:348c660ea6f6 65 void UserLoop(char n,const u8* data)
o2132613 17:348c660ea6f6 66 {
hirokimineshita 0:736c76a75def 67 u16 ButtonState;
o2132613 17:348c660ea6f6 68 if(n==0) { //有線Ps3USB.cpp
hirokimineshita 0:736c76a75def 69 RSX = ((ps3report*)data)->RightStickX;
hirokimineshita 0:736c76a75def 70 RSY = ((ps3report*)data)->RightStickY;
hirokimineshita 0:736c76a75def 71 LSX = ((ps3report*)data)->LeftStickX;
hirokimineshita 0:736c76a75def 72 LSY = ((ps3report*)data)->LeftStickY;
hirokimineshita 0:736c76a75def 73 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 74 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
hirokimineshita 0:736c76a75def 75 //ボタンの処理
hirokimineshita 0:736c76a75def 76 ButtonState = ((ps3report*)data)->ButtonState;
o2132613 17:348c660ea6f6 77 } else {//無線TestShell.cpp
hirokimineshita 0:736c76a75def 78 RSX = ((ps3report*)(data + 1))->RightStickX;
hirokimineshita 0:736c76a75def 79 RSY = ((ps3report*)(data + 1))->RightStickY;
hirokimineshita 0:736c76a75def 80 LSX = ((ps3report*)(data + 1))->LeftStickX;
hirokimineshita 0:736c76a75def 81 LSY = ((ps3report*)(data + 1))->LeftStickY;
hirokimineshita 0:736c76a75def 82 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 83 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
hirokimineshita 0:736c76a75def 84 //ボタンの処理
hirokimineshita 0:736c76a75def 85 ButtonState = ((ps3report*)(data + 1))->ButtonState;
hirokimineshita 0:736c76a75def 86 }
hirokimineshita 0:736c76a75def 87 //ここより下にプログラムを書く
o2132613 19:510b87211851 88 controller.frequency(1000000);
o2132613 19:510b87211851 89 led1=0;
o2132613 19:510b87211851 90 led2=0;
o2132613 19:510b87211851 91 led3=0;
o2132613 19:510b87211851 92 led4=0;
o2132613 19:510b87211851 93
o2132613 19:510b87211851 94 //move
o2132613 19:510b87211851 95 if((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //UPL1
o2132613 19:510b87211851 96 move = 9;
o2132613 19:510b87211851 97 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //DownR1
o2132613 19:510b87211851 98 move = 3;
o2132613 19:510b87211851 99 } else if ((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //UPR1
o2132613 19:510b87211851 100 move = 7;
o2132613 19:510b87211851 101 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //DownL1
o2132613 19:510b87211851 102 move = 1;
o2132613 19:510b87211851 103 } else if((ButtonState >> BUTTONUP)&1 == 1) { //up
o2132613 19:510b87211851 104 move = 8;
o2132613 19:510b87211851 105 led1 = 1;
o2132613 19:510b87211851 106 } else if((ButtonState >> BUTTONDOWN)&1 == 1) { //down
o2132613 19:510b87211851 107 move = 2;
o2132613 19:510b87211851 108 led2 =1;
o2132613 19:510b87211851 109 } else if((ButtonState >> BUTTONRIGHT)&1 == 1) { //right
o2132613 19:510b87211851 110 move = 6;
o2132613 19:510b87211851 111 led3 = 1;
o2132613 19:510b87211851 112 } else if((ButtonState >> BUTTONLEFT)&1 == 1) { //left
o2132613 19:510b87211851 113 move = 4;
o2132613 19:510b87211851 114 led4 = 1;
o2132613 19:510b87211851 115 } else if((ButtonState >> BUTTONL1)&1 == 1) { //L1
o2132613 19:510b87211851 116 move = 10;
o2132613 19:510b87211851 117 } else if((ButtonState >> BUTTONR1)&1 == 1) { //R1
o2132613 19:510b87211851 118 move = 11;
o2132613 19:510b87211851 119 } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
o2132613 19:510b87211851 120 move = 1;
o2132613 19:510b87211851 121 } else {
o2132613 19:510b87211851 122 move = 5;
o2132613 19:510b87211851 123 }
o2132613 19:510b87211851 124 //move
o2132613 19:510b87211851 125 //shoot
o2132613 19:510b87211851 126 if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
o2132613 19:510b87211851 127 shoot = 1;
o2132613 19:510b87211851 128 led1 = 1;
o2132613 19:510b87211851 129 } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
o2132613 22:f0e17f3129cc 130 ball = 1;
o2132613 19:510b87211851 131 led2 = 1;
o2132613 19:510b87211851 132 } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
o2132613 22:f0e17f3129cc 133 box = 1;
o2132613 19:510b87211851 134 led3 = 1;
o2132613 19:510b87211851 135 } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
o2132613 22:f0e17f3129cc 136 ball = 2;
maxnagazumi 20:dd4634500bb3 137 led4 = 1;
maxnagazumi 20:dd4634500bb3 138 } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
o2132613 22:f0e17f3129cc 139 box = 2;
o2132613 19:510b87211851 140 led4 = 1;
o2132613 17:348c660ea6f6 141 }
maxnagazumi 20:dd4634500bb3 142 show_angle();
o2132613 19:510b87211851 143 //CAN通信用プログラム
o2132613 19:510b87211851 144 if(shoot==1)shot();
o2132613 19:510b87211851 145 if(shoot>=2)catch_ball();
o2132613 19:510b87211851 146 can[0] = move;
o2132613 19:510b87211851 147 controller.write(CANMessage(1,can,1));
o2132613 19:510b87211851 148 printf("%d\r\n",can[0]);
o2132613 19:510b87211851 149 }
o2132613 19:510b87211851 150
maxnagazumi 20:dd4634500bb3 151 void show_angle()
maxnagazumi 20:dd4634500bb3 152 {
maxnagazumi 20:dd4634500bb3 153 X.rise(&Xcount);
maxnagazumi 20:dd4634500bb3 154 f.period_us(50);
maxnagazumi 20:dd4634500bb3 155 b.period_us(50);
maxnagazumi 20:dd4634500bb3 156 double a=0,r=0.4,v=0;
maxnagazumi 20:dd4634500bb3 157 int i=0,count;//rで半径指定 a*r=v
maxnagazumi 20:dd4634500bb3 158 //ticker.attach(&cal,0.05);
maxnagazumi 20:dd4634500bb3 159 if(X_count ==1) {
maxnagazumi 20:dd4634500bb3 160 printf("set ok");
maxnagazumi 20:dd4634500bb3 161 EC.reset();
maxnagazumi 20:dd4634500bb3 162 }
maxnagazumi 20:dd4634500bb3 163 //角度リセット
maxnagazumi 20:dd4634500bb3 164 a=EC.getOmega();
maxnagazumi 20:dd4634500bb3 165 count=EC.getCount();
maxnagazumi 20:dd4634500bb3 166 i =count%500;
maxnagazumi 20:dd4634500bb3 167 printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count);
maxnagazumi 20:dd4634500bb3 168 f=0;
maxnagazumi 20:dd4634500bb3 169 b=0;
maxnagazumi 20:dd4634500bb3 170 led1=0;
maxnagazumi 20:dd4634500bb3 171 led2=1;
maxnagazumi 20:dd4634500bb3 172 led3=1;
maxnagazumi 20:dd4634500bb3 173 led4=0;
maxnagazumi 20:dd4634500bb3 174
maxnagazumi 20:dd4634500bb3 175 }
o2132613 19:510b87211851 176 void shot()
o2132613 19:510b87211851 177 {
o2132613 19:510b87211851 178 X.rise(&Xcount);
o2132613 19:510b87211851 179 f.period_us(50);
o2132613 19:510b87211851 180 b.period_us(50);
o2132613 19:510b87211851 181 double a=0,r=0.4,v=0;
o2132613 19:510b87211851 182 int i=0,count;//rで半径指定 a*r=v
o2132613 19:510b87211851 183 ticker.attach(&cal,0.05);
maxnagazumi 20:dd4634500bb3 184 while(1) {
maxnagazumi 20:dd4634500bb3 185 printf("set");
maxnagazumi 20:dd4634500bb3 186 if(X_count ==1) {
maxnagazumi 20:dd4634500bb3 187 EC.reset();
maxnagazumi 20:dd4634500bb3 188 X_count =0;
maxnagazumi 20:dd4634500bb3 189 }
maxnagazumi 20:dd4634500bb3 190 break;
o2132613 17:348c660ea6f6 191 }
o2132613 19:510b87211851 192 //角度リセット
o2132613 19:510b87211851 193 while(1) {
o2132613 19:510b87211851 194 printf("%.3f %.3f %d %d\r\n",a,v,X_count,i);
o2132613 19:510b87211851 195 a=EC.getOmega();
o2132613 19:510b87211851 196 count=EC.getCount();
o2132613 19:510b87211851 197 i =count%500;
o2132613 19:510b87211851 198 v=a*r;
o2132613 19:510b87211851 199 f=0.45;//速度一定
o2132613 19:510b87211851 200 b=0.0;
o2132613 19:510b87211851 201 led1=1;
o2132613 19:510b87211851 202 led2=0;
o2132613 19:510b87211851 203 led3=0;
o2132613 19:510b87211851 204 led4=1;
o2132613 19:510b87211851 205 if(X_count>3) {
o2132613 19:510b87211851 206 if(i < -380 && i > -390) {//離す角度を決める
o2132613 19:510b87211851 207 out2=1; //ボールを離す
o2132613 19:510b87211851 208 printf("shot\r\n");
o2132613 19:510b87211851 209 led1=1;
o2132613 19:510b87211851 210 led2=1;
o2132613 19:510b87211851 211 led3=1;
o2132613 19:510b87211851 212 led4=1;
o2132613 19:510b87211851 213 break;
o2132613 19:510b87211851 214 }
o2132613 19:510b87211851 215 }
o2132613 17:348c660ea6f6 216 }
o2132613 19:510b87211851 217 printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i);
o2132613 19:510b87211851 218 X.rise(&Xcount);
o2132613 19:510b87211851 219 a=EC.getOmega();
o2132613 19:510b87211851 220 count=EC.getCount();
o2132613 19:510b87211851 221 i =count%500;
o2132613 19:510b87211851 222 f=0;
o2132613 19:510b87211851 223 b=0;
o2132613 19:510b87211851 224 led1=0;
o2132613 19:510b87211851 225 led2=1;
o2132613 19:510b87211851 226 led3=1;
o2132613 19:510b87211851 227 led4=0;
o2132613 19:510b87211851 228
o2132613 19:510b87211851 229 }
o2132613 19:510b87211851 230
o2132613 19:510b87211851 231 void catch_ball()
o2132613 19:510b87211851 232 {
o2132613 22:f0e17f3129cc 233
o2132613 22:f0e17f3129cc 234 switch (ball) {
o2132613 22:f0e17f3129cc 235 case 1: //ball servo
o2132613 22:f0e17f3129cc 236 if(ball_count ==0) {
o2132613 23:0c6bc29c8f86 237 servo.set_degree(id, first_ball);
maxnagazumi 20:dd4634500bb3 238 wait(0.5);
o2132613 23:0c6bc29c8f86 239 ball_count =1;
o2132613 23:0c6bc29c8f86 240 ball = 0;
o2132613 22:f0e17f3129cc 241 } else if(ball_count ==1) {
o2132613 22:f0e17f3129cc 242 servo.set_degree(id, grab_ball);
maxnagazumi 20:dd4634500bb3 243 wait(0.5);
o2132613 23:0c6bc29c8f86 244 ball_count = 0;
maxnagazumi 20:dd4634500bb3 245 ball =0;
maxnagazumi 20:dd4634500bb3 246 }
maxnagazumi 20:dd4634500bb3 247 break;
o2132613 22:f0e17f3129cc 248 case 2://ball air
maxnagazumi 20:dd4634500bb3 249 if (out1==0) {
maxnagazumi 20:dd4634500bb3 250 out1=1;
o2132613 23:0c6bc29c8f86 251 ball=0;
maxnagazumi 20:dd4634500bb3 252 } else if (out1 == 1) {
maxnagazumi 20:dd4634500bb3 253 out1=0;
o2132613 23:0c6bc29c8f86 254 ball=0;
maxnagazumi 20:dd4634500bb3 255 }
o2132613 19:510b87211851 256 }
o2132613 19:510b87211851 257 }
o2132613 19:510b87211851 258
o2132613 19:510b87211851 259 void catch_box()
o2132613 19:510b87211851 260 {
o2132613 22:f0e17f3129cc 261 switch(box) {
o2132613 22:f0e17f3129cc 262 case 1:// box servo
o2132613 22:f0e17f3129cc 263 if(box_count ==0) {
o2132613 23:0c6bc29c8f86 264 servo.set_degree(id2, first_box);
o2132613 22:f0e17f3129cc 265 wait(0.5);
o2132613 23:0c6bc29c8f86 266 box_count =1;
o2132613 23:0c6bc29c8f86 267 box = 1;
o2132613 22:f0e17f3129cc 268 } else if(box_count ==1) {
o2132613 22:f0e17f3129cc 269 servo.set_degree(id2, grab_box);
o2132613 22:f0e17f3129cc 270 wait(0.5);
o2132613 23:0c6bc29c8f86 271 box_count = 0;
o2132613 23:0c6bc29c8f86 272 box =0;
o2132613 22:f0e17f3129cc 273 }
o2132613 22:f0e17f3129cc 274 case 2: //box air
o2132613 22:f0e17f3129cc 275 if (out3==0) {
o2132613 22:f0e17f3129cc 276 out3=1;
o2132613 23:0c6bc29c8f86 277 box = 0;
o2132613 22:f0e17f3129cc 278 } else if (out3 == 1) {
o2132613 22:f0e17f3129cc 279 out3=0;
o2132613 23:0c6bc29c8f86 280 box = 0;
o2132613 22:f0e17f3129cc 281 }
o2132613 22:f0e17f3129cc 282 }
hirokimineshita 0:736c76a75def 283 }