手動bのほぼ全部
Dependents: kobayashi_rei 2019BSyudo Bsyudo 2019Bsyudo
Diff: states.cpp
- Revision:
- 5:3273db2411d8
- Parent:
- 4:eafd5a0c3351
- Child:
- 7:9d5746931f80
diff -r eafd5a0c3351 -r 3273db2411d8 states.cpp --- a/states.cpp Mon Sep 09 04:48:16 2019 +0000 +++ b/states.cpp Tue Sep 10 01:49:31 2019 +0000 @@ -38,16 +38,16 @@ for(int i=0; i<4; i++) stick[i] = pad.getStick(i); stick[1] = 255-stick[1]; stick[3] = 255-stick[3]; - norm[0] = hypot(stick[0],stick[1]); - norm[1] = hypot(stick[2],stick[3]); + norm[0] = hypotf(stick[0],stick[1]); + norm[1] = hypotf(stick[2],stick[3]); if(norm[0] > 20) norm[0] = 0; if(norm[1] > 20) norm[1] = 0; for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i); omni.setBrake(trigger[0]); nano.sendTrigger(trigger[0]); mechanism.getTrigger(trigger[1]); - rad[0] = atan2(stick[1],stick[0]) + (nowAngle * (PI / 180)); - rad[1] = atan2(stick[2],stick[3]); + rad[0] = atan2f(stick[1],stick[0]) + (nowAngle * (PI / 180)); + rad[1] = atan2f(stick[2],stick[3]); if(b3[4] == 1) state++; if(b3[5] == 1) state--; /* @@ -63,21 +63,6 @@ case 3: idealAngle = left; break; default: idealAngle = front; break; } - /*昇降機構*/ - if(b[0] && check == 1) mechanism.up(); - else if(b[1] && check == 1) mechanism.down(); - /*回収機構*/ - else if(b[6] && check == 1) mechanism.collect(); - /*つり上げ機構*/ - else if(b[7] && check == 1) mechanism.lift(); - /*装填機構*/ - else if(b[2] && check == 1) mechanism.loadRight(); - else if(b[3]) mechanism.loadLeft(); - /*かけ機構*/ - else if(b[9] && check == 1) mechanism.hookRight(); - else if(b[8] && check == 1) mechanism.hookLeft(); - - else nano.sendMove(0x0f); } void States::move() @@ -96,5 +81,20 @@ turnPower = anglePID.compute(); if(norm[0] && norm[0] < 200) omni.stop(); else omni.move(stick[0], stick[1], -turnPower); + /*昇降機構*/ + if(b[0] && check == 1) mechanism.up(); + else if(b[1] && check == 1) mechanism.down(); + /*回収機構*/ + else if(b[6] && check == 1) mechanism.collect(); + /*つり上げ機構*/ + else if(b[7] && check == 1) mechanism.lift(); + /*装填機構*/ + else if(b[2] && check == 1) mechanism.loadRight(); + else if(b[3]) mechanism.loadLeft(); + /*かけ機構*/ + else if(b[9] && check == 1) mechanism.hookRight(); + else if(b[8] && check == 1) mechanism.hookLeft(); + + else nano.sendMove(0x0f); } } \ No newline at end of file