test
Dependencies: mbed KondoServoLibrary Encoder
Diff: User.cpp
- Revision:
- 20:dd4634500bb3
- Parent:
- 19:510b87211851
- Child:
- 21:0f12ee2322e4
--- a/User.cpp Wed Mar 11 09:08:05 2020 +0000 +++ b/User.cpp Thu Mar 12 03:04:51 2020 +0000 @@ -32,18 +32,20 @@ DigitalOut out2(p20);//エアシリンダー(投擲) - int id = 0; +int id = 0; double SERVO2DEG = 270.0 / (11500 - 3500); double first = (6800 - 3500) * SERVO2DEG; -double grab = (3800 - 3500) * SERVO2DEG; +double grab = (3600 - 3500) * SERVO2DEG; double pass = (5000 - 3500) * SERVO2DEG; void cal() { EC.calOmega(); } + void shot(); void catch_ball(); +void show_angle(); int X_count=0; void Xcount() @@ -119,15 +121,19 @@ shoot = 1; led1 = 1; } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X - shoot = 0; + shoot = 4; led2 = 1; } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△ shoot = 3; led3 = 1; } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□ - shoot = 4; + shoot = 2; + led4 = 1; + } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2 + shoot = 5; led4 = 1; } + show_angle(); //CAN通信用プログラム if(shoot==1)shot(); if(shoot>=2)catch_ball(); @@ -136,6 +142,31 @@ printf("%d\r\n",can[0]); } +void show_angle() +{ + 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); + if(X_count ==1) { + printf("set ok"); + EC.reset(); + } + //角度リセット + a=EC.getOmega(); + count=EC.getCount(); + i =count%500; + printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count); + f=0; + b=0; + led1=0; + led2=1; + led3=1; + led4=0; + +} void shot() { X.rise(&Xcount); @@ -144,10 +175,13 @@ double a=0,r=0.4,v=0; int i=0,count;//rで半径指定 a*r=v ticker.attach(&cal,0.05); - printf("set"); - if(X_count ==1) { - EC.reset(); - X_count =0; + while(1) { + printf("set"); + if(X_count ==1) { + EC.reset(); + X_count =0; + } + break; } //角度リセット while(1) { @@ -192,17 +226,42 @@ { switch (shoot) { case 2: - servo.set_degree(id, pass); - break; + if(ball ==0) { + servo.set_degree(id, first); + wait(0.5); + shoot =0; + ball = 1; + } else if(ball ==1) { + servo.set_degree(id, grab); + wait(0.5); + shoot = 0; + ball =0; + } + break; case 3: - servo.set_degree(id, grab); - break; + f=0.01; + wait (0.1); + f=0; + shoot = 0; + break; case 4: - if (out1==0){ - out1=1; - }else if (out1 == 1){ - out1=0; - } + if (out1==0) { + out1=1; + shoot=0; + } else if (out1 == 1) { + out1=0; + shoot=0; + } + break; + case 5: + if (out2==0) { + out2=1; + shoot=0; + } else if (out2 == 1) { + out2=0; + shoot=0; + } + } }