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

test

Dependencies:   mbed KondoServoLibrary Encoder

User.cpp@20:dd4634500bb3, 2020-03-12 (annotated)

Committer:
maxnagazumi
Date:
Thu Mar 12 03:04:51 2020 +0000
Revision:
20:dd4634500bb3
Parent:
19:510b87211851
Child:
21:0f12ee2322e4
harurobo

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