another

Dependencies:   mbed KondoServoLibrary Encoder

Committer:
maxnagazumi
Date:
Thu Mar 19 06:18:37 2020 +0000
Revision:
24:674b011da45b
Parent:
23:0c6bc29c8f86
Child:
25:4eb2f2795120
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 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
maxnagazumi 24:674b011da45b 34 DigitalIn touchSensor(p14);//タッチセンサー
maxnagazumi 24:674b011da45b 35
o2132613 19:510b87211851 36 DigitalOut out1(p19);//エアシリンダー(つかむ方)
maxnagazumi 24:674b011da45b 37 DigitalOut out2(p20);//エアシリンダー(投擲))
koheim 21:0f12ee2322e4 38 DigitalOut out3(p18);//エアシリンダー(ボックス)
baba2357 11:86d717718dbf 39
maxnagazumi 24:674b011da45b 40 int id = 1;//ballbreak;
maxnagazumi 24:674b011da45b 41 int id2 = 0;//box
o2132613 19:510b87211851 42 double SERVO2DEG = 270.0 / (11500 - 3500);
maxnagazumi 24:674b011da45b 43 double first_ball = (5000 - 3500) * SERVO2DEG;
maxnagazumi 24:674b011da45b 44 double first_box = (5000 - 3500) * SERVO2DEG; //X haven't be decided
maxnagazumi 24:674b011da45b 45 double grab_ball = (3500 - 3500) * SERVO2DEG;
maxnagazumi 24:674b011da45b 46 double pass = (8000 - 3500) * SERVO2DEG;
maxnagazumi 24:674b011da45b 47 double grab_box = (3500 - 3500) * SERVO2DEG;
o2132613 19:510b87211851 48
o2132613 19:510b87211851 49 void cal()
o2132613 19:510b87211851 50 {
o2132613 19:510b87211851 51 EC.calOmega();
o2132613 19:510b87211851 52 }
maxnagazumi 20:dd4634500bb3 53
o2132613 19:510b87211851 54 void shot();
o2132613 19:510b87211851 55 void catch_ball();
maxnagazumi 20:dd4634500bb3 56 void show_angle();
o2132613 19:510b87211851 57
o2132613 19:510b87211851 58 int X_count=0;
o2132613 19:510b87211851 59 void Xcount()
o2132613 19:510b87211851 60 {
o2132613 19:510b87211851 61 X_count++;
o2132613 19:510b87211851 62 }
o2132613 19:510b87211851 63
o2132613 17:348c660ea6f6 64 void UserLoopSetting()
o2132613 17:348c660ea6f6 65 {
maxnagazumi 24:674b011da45b 66 f.period_us(50);
maxnagazumi 24:674b011da45b 67 b.period_us(50);
maxnagazumi 24:674b011da45b 68 touchSensor.mode(PullUp);
hirokimineshita 0:736c76a75def 69 }
o2132613 17:348c660ea6f6 70 void UserLoop(char n,const u8* data)
o2132613 17:348c660ea6f6 71 {
maxnagazumi 24:674b011da45b 72 static int i=0;
maxnagazumi 24:674b011da45b 73 if(i==0) {
maxnagazumi 24:674b011da45b 74 wait(2);
maxnagazumi 24:674b011da45b 75 i++;
maxnagazumi 24:674b011da45b 76 }
hirokimineshita 0:736c76a75def 77 u16 ButtonState;
o2132613 17:348c660ea6f6 78 if(n==0) { //有線Ps3USB.cpp
hirokimineshita 0:736c76a75def 79 RSX = ((ps3report*)data)->RightStickX;
hirokimineshita 0:736c76a75def 80 RSY = ((ps3report*)data)->RightStickY;
hirokimineshita 0:736c76a75def 81 LSX = ((ps3report*)data)->LeftStickX;
hirokimineshita 0:736c76a75def 82 LSY = ((ps3report*)data)->LeftStickY;
hirokimineshita 0:736c76a75def 83 BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 84 BSL = (u8)(((ps3report*)data)->ButtonState >> 8);
hirokimineshita 0:736c76a75def 85 //ボタンの処理
hirokimineshita 0:736c76a75def 86 ButtonState = ((ps3report*)data)->ButtonState;
o2132613 17:348c660ea6f6 87 } else {//無線TestShell.cpp
hirokimineshita 0:736c76a75def 88 RSX = ((ps3report*)(data + 1))->RightStickX;
hirokimineshita 0:736c76a75def 89 RSY = ((ps3report*)(data + 1))->RightStickY;
hirokimineshita 0:736c76a75def 90 LSX = ((ps3report*)(data + 1))->LeftStickX;
hirokimineshita 0:736c76a75def 91 LSY = ((ps3report*)(data + 1))->LeftStickY;
hirokimineshita 0:736c76a75def 92 BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff);
hirokimineshita 0:736c76a75def 93 BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8);
hirokimineshita 0:736c76a75def 94 //ボタンの処理
hirokimineshita 0:736c76a75def 95 ButtonState = ((ps3report*)(data + 1))->ButtonState;
hirokimineshita 0:736c76a75def 96 }
hirokimineshita 0:736c76a75def 97 //ここより下にプログラムを書く
o2132613 19:510b87211851 98 controller.frequency(1000000);
o2132613 19:510b87211851 99 led1=0;
o2132613 19:510b87211851 100 led2=0;
o2132613 19:510b87211851 101 led3=0;
o2132613 19:510b87211851 102 led4=0;
o2132613 19:510b87211851 103
o2132613 19:510b87211851 104 //move
o2132613 19:510b87211851 105 if((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //UPL1
o2132613 19:510b87211851 106 move = 9;
o2132613 19:510b87211851 107 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //DownR1
o2132613 19:510b87211851 108 move = 3;
o2132613 19:510b87211851 109 } else if ((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //UPR1
o2132613 19:510b87211851 110 move = 7;
o2132613 19:510b87211851 111 } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //DownL1
o2132613 19:510b87211851 112 move = 1;
o2132613 19:510b87211851 113 } else if((ButtonState >> BUTTONUP)&1 == 1) { //up
o2132613 19:510b87211851 114 move = 8;
o2132613 19:510b87211851 115 led1 = 1;
o2132613 19:510b87211851 116 } else if((ButtonState >> BUTTONDOWN)&1 == 1) { //down
o2132613 19:510b87211851 117 move = 2;
o2132613 19:510b87211851 118 led2 =1;
o2132613 19:510b87211851 119 } else if((ButtonState >> BUTTONRIGHT)&1 == 1) { //right
o2132613 19:510b87211851 120 move = 6;
o2132613 19:510b87211851 121 led3 = 1;
o2132613 19:510b87211851 122 } else if((ButtonState >> BUTTONLEFT)&1 == 1) { //left
o2132613 19:510b87211851 123 move = 4;
o2132613 19:510b87211851 124 led4 = 1;
o2132613 19:510b87211851 125 } else if((ButtonState >> BUTTONL1)&1 == 1) { //L1
o2132613 19:510b87211851 126 move = 10;
o2132613 19:510b87211851 127 } else if((ButtonState >> BUTTONR1)&1 == 1) { //R1
o2132613 19:510b87211851 128 move = 11;
maxnagazumi 24:674b011da45b 129 } else if((ButtonState >> BUTTONSTART)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
maxnagazumi 24:674b011da45b 130 move = 12;
o2132613 19:510b87211851 131 } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
maxnagazumi 24:674b011da45b 132 move = 13;
o2132613 19:510b87211851 133 } else {
o2132613 19:510b87211851 134 move = 5;
o2132613 19:510b87211851 135 }
o2132613 19:510b87211851 136 //move
o2132613 19:510b87211851 137 //shoot
o2132613 19:510b87211851 138 if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
o2132613 19:510b87211851 139 shoot = 1;
maxnagazumi 24:674b011da45b 140 printf("shoot 1\r\n");
o2132613 19:510b87211851 141 led1 = 1;
o2132613 19:510b87211851 142 } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
o2132613 22:f0e17f3129cc 143 ball = 1;
maxnagazumi 24:674b011da45b 144 printf("ball 1\r\n");
o2132613 19:510b87211851 145 led2 = 1;
o2132613 19:510b87211851 146 } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
o2132613 22:f0e17f3129cc 147 box = 1;
maxnagazumi 24:674b011da45b 148 printf("box 1\r\n");
o2132613 19:510b87211851 149 led3 = 1;
o2132613 19:510b87211851 150 } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
o2132613 22:f0e17f3129cc 151 ball = 2;
maxnagazumi 24:674b011da45b 152 printf("ball 2\r\n");
maxnagazumi 20:dd4634500bb3 153 led4 = 1;
maxnagazumi 20:dd4634500bb3 154 } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
o2132613 22:f0e17f3129cc 155 box = 2;
maxnagazumi 24:674b011da45b 156 printf("box 2\r\n");
o2132613 19:510b87211851 157 led4 = 1;
o2132613 17:348c660ea6f6 158 }
maxnagazumi 20:dd4634500bb3 159 show_angle();
o2132613 19:510b87211851 160 //CAN通信用プログラム
o2132613 19:510b87211851 161 if(shoot==1)shot();
maxnagazumi 24:674b011da45b 162 catch_ball();
o2132613 19:510b87211851 163 can[0] = move;
o2132613 19:510b87211851 164 controller.write(CANMessage(1,can,1));
o2132613 19:510b87211851 165 printf("%d\r\n",can[0]);
o2132613 19:510b87211851 166 }
o2132613 19:510b87211851 167
maxnagazumi 20:dd4634500bb3 168 void show_angle()
maxnagazumi 20:dd4634500bb3 169 {
maxnagazumi 20:dd4634500bb3 170 X.rise(&Xcount);
maxnagazumi 20:dd4634500bb3 171 f.period_us(50);
maxnagazumi 20:dd4634500bb3 172 b.period_us(50);
maxnagazumi 20:dd4634500bb3 173 double a=0,r=0.4,v=0;
maxnagazumi 20:dd4634500bb3 174 int i=0,count;//rで半径指定 a*r=v
maxnagazumi 20:dd4634500bb3 175 //ticker.attach(&cal,0.05);
maxnagazumi 20:dd4634500bb3 176 if(X_count ==1) {
maxnagazumi 20:dd4634500bb3 177 printf("set ok");
maxnagazumi 20:dd4634500bb3 178 EC.reset();
maxnagazumi 20:dd4634500bb3 179 }
maxnagazumi 20:dd4634500bb3 180 //角度リセット
maxnagazumi 20:dd4634500bb3 181 a=EC.getOmega();
maxnagazumi 20:dd4634500bb3 182 count=EC.getCount();
maxnagazumi 20:dd4634500bb3 183 i =count%500;
maxnagazumi 20:dd4634500bb3 184 printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count);
maxnagazumi 20:dd4634500bb3 185 f=0;
maxnagazumi 20:dd4634500bb3 186 b=0;
maxnagazumi 20:dd4634500bb3 187 led1=0;
maxnagazumi 20:dd4634500bb3 188 led2=1;
maxnagazumi 20:dd4634500bb3 189 led3=1;
maxnagazumi 20:dd4634500bb3 190 led4=0;
maxnagazumi 20:dd4634500bb3 191
maxnagazumi 20:dd4634500bb3 192 }
o2132613 19:510b87211851 193 void shot()
o2132613 19:510b87211851 194 {
maxnagazumi 24:674b011da45b 195 out1=1;
maxnagazumi 24:674b011da45b 196 int breaknum=0;
o2132613 19:510b87211851 197 X.rise(&Xcount);
o2132613 19:510b87211851 198 double a=0,r=0.4,v=0;
o2132613 19:510b87211851 199 int i=0,count;//rで半径指定 a*r=v
o2132613 19:510b87211851 200 ticker.attach(&cal,0.05);
maxnagazumi 20:dd4634500bb3 201 while(1) {
maxnagazumi 24:674b011da45b 202 printf("%d\r\n",touchSensor.read());
maxnagazumi 24:674b011da45b 203 f=0;
maxnagazumi 24:674b011da45b 204 b=0.05;
maxnagazumi 24:674b011da45b 205 if(touchSensor==0) {//受け渡し
maxnagazumi 24:674b011da45b 206 f=0;
maxnagazumi 24:674b011da45b 207 b=0;
maxnagazumi 24:674b011da45b 208 printf("%d",EC.getCount());
maxnagazumi 20:dd4634500bb3 209 EC.reset();
maxnagazumi 24:674b011da45b 210 printf("%d",EC.getCount());
maxnagazumi 24:674b011da45b 211 wait(2);
maxnagazumi 24:674b011da45b 212 out1=0;
maxnagazumi 24:674b011da45b 213 wait(1);
maxnagazumi 24:674b011da45b 214 out2=0;
maxnagazumi 24:674b011da45b 215 wait(1);
maxnagazumi 24:674b011da45b 216 printf("passed\r\n");
maxnagazumi 24:674b011da45b 217 wait(1);
maxnagazumi 24:674b011da45b 218 while(1) {
maxnagazumi 24:674b011da45b 219 f=0.1;
maxnagazumi 24:674b011da45b 220 b=0;
maxnagazumi 24:674b011da45b 221 printf("moving to remove servo\r\n");
maxnagazumi 24:674b011da45b 222 if(EC.getCount()>200) {
maxnagazumi 24:674b011da45b 223 printf("servo move\r\n");
maxnagazumi 24:674b011da45b 224 f=0;
maxnagazumi 24:674b011da45b 225 b=0;
maxnagazumi 24:674b011da45b 226 servo.set_degree(id, first_ball);
maxnagazumi 24:674b011da45b 227 breaknum=1;
maxnagazumi 24:674b011da45b 228 break;
maxnagazumi 24:674b011da45b 229 }
maxnagazumi 24:674b011da45b 230
maxnagazumi 24:674b011da45b 231 }
maxnagazumi 24:674b011da45b 232 if(X_count ==1) {
maxnagazumi 24:674b011da45b 233 EC.reset();
maxnagazumi 24:674b011da45b 234 X_count =0;
maxnagazumi 24:674b011da45b 235 }
maxnagazumi 20:dd4634500bb3 236 }
maxnagazumi 24:674b011da45b 237 if(breaknum==1)break;
o2132613 17:348c660ea6f6 238 }
maxnagazumi 24:674b011da45b 239 EC.reset();
maxnagazumi 24:674b011da45b 240 wait(1);
maxnagazumi 24:674b011da45b 241 X_count=0;
maxnagazumi 24:674b011da45b 242 double m=0;
o2132613 19:510b87211851 243 //角度リセット
o2132613 19:510b87211851 244 while(1) {
maxnagazumi 24:674b011da45b 245 //printf("%.3f %d %d\r\n",v,X_count,i);
maxnagazumi 24:674b011da45b 246 //a=EC.getOmega();
o2132613 19:510b87211851 247 count=EC.getCount();
o2132613 19:510b87211851 248 i =count%500;
maxnagazumi 24:674b011da45b 249 //v=a*r;
maxnagazumi 24:674b011da45b 250 if(m<1.0) {
maxnagazumi 24:674b011da45b 251 m=m+0.05;
maxnagazumi 24:674b011da45b 252 }
maxnagazumi 24:674b011da45b 253 if(m>1) {
maxnagazumi 24:674b011da45b 254 m=1;
maxnagazumi 24:674b011da45b 255 }
maxnagazumi 24:674b011da45b 256 f=0.45*m;//速度一定
o2132613 19:510b87211851 257 b=0.0;
maxnagazumi 24:674b011da45b 258 if(X_count>2) {
maxnagazumi 24:674b011da45b 259 if(i > 345 && i < 355) {//離す角度を決める
maxnagazumi 24:674b011da45b 260 out1=1; //ボールを離す
maxnagazumi 24:674b011da45b 261 f=0;
maxnagazumi 24:674b011da45b 262 b=0;
maxnagazumi 24:674b011da45b 263 wait(1);
o2132613 19:510b87211851 264 printf("shot\r\n");
o2132613 19:510b87211851 265 led1=1;
o2132613 19:510b87211851 266 led2=1;
o2132613 19:510b87211851 267 led3=1;
o2132613 19:510b87211851 268 led4=1;
o2132613 19:510b87211851 269 break;
o2132613 19:510b87211851 270 }
o2132613 19:510b87211851 271 }
o2132613 17:348c660ea6f6 272 }
maxnagazumi 24:674b011da45b 273 printf("%.3f %.3f %d\r\n",a,v,i);
o2132613 19:510b87211851 274 X.rise(&Xcount);
o2132613 19:510b87211851 275 a=EC.getOmega();
o2132613 19:510b87211851 276 count=EC.getCount();
o2132613 19:510b87211851 277 i =count%500;
o2132613 19:510b87211851 278 f=0;
o2132613 19:510b87211851 279 b=0;
o2132613 19:510b87211851 280 led1=0;
o2132613 19:510b87211851 281 led2=1;
o2132613 19:510b87211851 282 led3=1;
o2132613 19:510b87211851 283 led4=0;
maxnagazumi 24:674b011da45b 284 shoot=0;
o2132613 19:510b87211851 285 }
o2132613 19:510b87211851 286 void catch_ball()
o2132613 19:510b87211851 287 {
o2132613 22:f0e17f3129cc 288 switch (ball) {
o2132613 22:f0e17f3129cc 289 case 1: //ball servo
o2132613 22:f0e17f3129cc 290 if(ball_count ==0) {
maxnagazumi 24:674b011da45b 291 printf("pass\r\n");
maxnagazumi 24:674b011da45b 292 servo.set_degree(id,pass);
maxnagazumi 20:dd4634500bb3 293 wait(0.5);
o2132613 23:0c6bc29c8f86 294 ball_count =1;
o2132613 23:0c6bc29c8f86 295 ball = 0;
o2132613 22:f0e17f3129cc 296 } else if(ball_count ==1) {
maxnagazumi 24:674b011da45b 297 printf("glab\r\n");
maxnagazumi 24:674b011da45b 298 servo.set_degree(id, first_ball);
maxnagazumi 24:674b011da45b 299 wait(0.5);
maxnagazumi 24:674b011da45b 300 ball_count = 2;
maxnagazumi 24:674b011da45b 301 ball =0;
maxnagazumi 24:674b011da45b 302 } else if(ball_count ==2) {
maxnagazumi 24:674b011da45b 303 printf("first");
o2132613 22:f0e17f3129cc 304 servo.set_degree(id, grab_ball);
maxnagazumi 20:dd4634500bb3 305 wait(0.5);
o2132613 23:0c6bc29c8f86 306 ball_count = 0;
maxnagazumi 20:dd4634500bb3 307 ball =0;
maxnagazumi 20:dd4634500bb3 308 }
maxnagazumi 20:dd4634500bb3 309 break;
o2132613 22:f0e17f3129cc 310 case 2://ball air
maxnagazumi 20:dd4634500bb3 311 if (out1==0) {
maxnagazumi 20:dd4634500bb3 312 out1=1;
o2132613 23:0c6bc29c8f86 313 ball=0;
maxnagazumi 20:dd4634500bb3 314 } else if (out1 == 1) {
maxnagazumi 20:dd4634500bb3 315 out1=0;
o2132613 23:0c6bc29c8f86 316 ball=0;
maxnagazumi 20:dd4634500bb3 317 }
maxnagazumi 24:674b011da45b 318 break;
o2132613 19:510b87211851 319 }
o2132613 19:510b87211851 320 }
o2132613 19:510b87211851 321
o2132613 19:510b87211851 322 void catch_box()
o2132613 19:510b87211851 323 {
o2132613 22:f0e17f3129cc 324 switch(box) {
o2132613 22:f0e17f3129cc 325 case 1:// box servo
o2132613 22:f0e17f3129cc 326 if(box_count ==0) {
maxnagazumi 24:674b011da45b 327 printf("Box first");
o2132613 23:0c6bc29c8f86 328 servo.set_degree(id2, first_box);
o2132613 22:f0e17f3129cc 329 wait(0.5);
o2132613 23:0c6bc29c8f86 330 box_count =1;
maxnagazumi 24:674b011da45b 331 box = 0;
o2132613 22:f0e17f3129cc 332 } else if(box_count ==1) {
maxnagazumi 24:674b011da45b 333 printf("Box grab");
o2132613 22:f0e17f3129cc 334 servo.set_degree(id2, grab_box);
o2132613 22:f0e17f3129cc 335 wait(0.5);
o2132613 23:0c6bc29c8f86 336 box_count = 0;
o2132613 23:0c6bc29c8f86 337 box =0;
o2132613 22:f0e17f3129cc 338 }
maxnagazumi 24:674b011da45b 339 break;
o2132613 22:f0e17f3129cc 340 case 2: //box air
o2132613 22:f0e17f3129cc 341 if (out3==0) {
o2132613 22:f0e17f3129cc 342 out3=1;
o2132613 23:0c6bc29c8f86 343 box = 0;
o2132613 22:f0e17f3129cc 344 } else if (out3 == 1) {
o2132613 22:f0e17f3129cc 345 out3=0;
o2132613 23:0c6bc29c8f86 346 box = 0;
o2132613 22:f0e17f3129cc 347 }
maxnagazumi 24:674b011da45b 348 break;
o2132613 22:f0e17f3129cc 349 }
hirokimineshita 0:736c76a75def 350 }