![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
version which is easier to read
Dependencies: mbed KondoServoLibrary Encoder
Diff: User.cpp
- Revision:
- 24:674b011da45b
- Parent:
- 23:0c6bc29c8f86
- Child:
- 25:4eb2f2795120
diff -r 0c6bc29c8f86 -r 674b011da45b User.cpp --- a/User.cpp Fri Mar 13 08:23:05 2020 +0000 +++ b/User.cpp Thu Mar 19 06:18:37 2020 +0000 @@ -31,18 +31,20 @@ Ec1multi EC(p16,p17,RESOLUTION);//投E InterruptIn X(p15); +DigitalIn touchSensor(p14);//タッチセンサー + DigitalOut out1(p19);//エアシリンダー(つかむ方) -DigitalOut out2(p20);//エアシリンダー(投擲) +DigitalOut out2(p20);//エアシリンダー(投擲)) DigitalOut out3(p18);//エアシリンダー(ボックス) -int id = 0; -int id2 = 1; +int id = 1;//ballbreak; +int id2 = 0;//box double SERVO2DEG = 270.0 / (11500 - 3500); -double first_ball = (6800 - 3500) * SERVO2DEG; -double first_box = (X - 3500) * SERVO2DEG; //X haven't be decided -double grab_ball = (3600 - 3500) * SERVO2DEG; -double pass = (5000 - 3500) * SERVO2DEG; -double grab_box = (3550 - 3500) * SERVO2DEG; +double first_ball = (5000 - 3500) * SERVO2DEG; +double first_box = (5000 - 3500) * SERVO2DEG; //X haven't be decided +double grab_ball = (3500 - 3500) * SERVO2DEG; +double pass = (8000 - 3500) * SERVO2DEG; +double grab_box = (3500 - 3500) * SERVO2DEG; void cal() { @@ -61,9 +63,17 @@ void UserLoopSetting() { + f.period_us(50); + b.period_us(50); + touchSensor.mode(PullUp); } void UserLoop(char n,const u8* data) { + static int i=0; + if(i==0) { + wait(2); + i++; + } u16 ButtonState; if(n==0) { //有線Ps3USB.cpp RSX = ((ps3report*)data)->RightStickX; @@ -116,8 +126,10 @@ move = 10; } else if((ButtonState >> BUTTONR1)&1 == 1) { //R1 move = 11; + } else if((ButtonState >> BUTTONSTART)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン + move = 12; } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン - move = 1; + move = 13; } else { move = 5; } @@ -125,24 +137,29 @@ //shoot if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O shoot = 1; + printf("shoot 1\r\n"); led1 = 1; } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X ball = 1; + printf("ball 1\r\n"); led2 = 1; } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△ box = 1; + printf("box 1\r\n"); led3 = 1; } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□ ball = 2; + printf("ball 2\r\n"); led4 = 1; } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2 box = 2; + printf("box 2\r\n"); led4 = 1; } show_angle(); //CAN通信用プログラム if(shoot==1)shot(); - if(shoot>=2)catch_ball(); + catch_ball(); can[0] = move; controller.write(CANMessage(1,can,1)); printf("%d\r\n",can[0]); @@ -175,36 +192,75 @@ } void shot() { + out1=1; + int breaknum=0; X.rise(&Xcount); - f.period_us(50); - b.period_us(50); double a=0,r=0.4,v=0; int i=0,count;//rで半径指定 a*r=v ticker.attach(&cal,0.05); while(1) { - printf("set"); - if(X_count ==1) { + printf("%d\r\n",touchSensor.read()); + f=0; + b=0.05; + if(touchSensor==0) {//受け渡し + f=0; + b=0; + printf("%d",EC.getCount()); EC.reset(); - X_count =0; + printf("%d",EC.getCount()); + wait(2); + out1=0; + wait(1); + out2=0; + wait(1); + printf("passed\r\n"); + wait(1); + while(1) { + f=0.1; + b=0; + printf("moving to remove servo\r\n"); + if(EC.getCount()>200) { + printf("servo move\r\n"); + f=0; + b=0; + servo.set_degree(id, first_ball); + breaknum=1; + break; + } + + } + if(X_count ==1) { + EC.reset(); + X_count =0; + } } - break; + if(breaknum==1)break; } + EC.reset(); + wait(1); + X_count=0; + double m=0; //角度リセット while(1) { - printf("%.3f %.3f %d %d\r\n",a,v,X_count,i); - a=EC.getOmega(); + //printf("%.3f %d %d\r\n",v,X_count,i); + //a=EC.getOmega(); count=EC.getCount(); i =count%500; - v=a*r; - f=0.45;//速度一定 + //v=a*r; + if(m<1.0) { + m=m+0.05; + } + if(m>1) { + m=1; + } + f=0.45*m;//速度一定 b=0.0; - led1=1; - led2=0; - led3=0; - led4=1; - if(X_count>3) { - if(i < -380 && i > -390) {//離す角度を決める - out2=1; //ボールを離す + if(X_count>2) { + if(i > 345 && i < 355) {//離す角度を決める + out1=1; //ボールを離す + f=0; + b=0; + wait(1); printf("shot\r\n"); led1=1; led2=1; @@ -214,7 +270,7 @@ } } } - printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i); + printf("%.3f %.3f %d\r\n",a,v,i); X.rise(&Xcount); a=EC.getOmega(); count=EC.getCount(); @@ -225,20 +281,26 @@ led2=1; led3=1; led4=0; - + shoot=0; } - void catch_ball() { - switch (ball) { case 1: //ball servo if(ball_count ==0) { - servo.set_degree(id, first_ball); + printf("pass\r\n"); + servo.set_degree(id,pass); wait(0.5); ball_count =1; ball = 0; } else if(ball_count ==1) { + printf("glab\r\n"); + servo.set_degree(id, first_ball); + wait(0.5); + ball_count = 2; + ball =0; + } else if(ball_count ==2) { + printf("first"); servo.set_degree(id, grab_ball); wait(0.5); ball_count = 0; @@ -253,6 +315,7 @@ out1=0; ball=0; } + break; } } @@ -261,16 +324,19 @@ switch(box) { case 1:// box servo if(box_count ==0) { + printf("Box first"); servo.set_degree(id2, first_box); wait(0.5); box_count =1; - box = 1; + box = 0; } else if(box_count ==1) { + printf("Box grab"); servo.set_degree(id2, grab_box); wait(0.5); box_count = 0; box =0; } + break; case 2: //box air if (out3==0) { out3=1; @@ -279,5 +345,6 @@ out3=0; box = 0; } + break; } } \ No newline at end of file