2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
Diff: main.cpp
- Revision:
- 13:d01356a29b93
- Parent:
- 12:9d21189a8eb0
- Child:
- 14:5cc44bec9cfc
diff -r 9d21189a8eb0 -r d01356a29b93 main.cpp --- a/main.cpp Wed Sep 13 22:58:42 2017 +0700 +++ b/main.cpp Thu Oct 05 16:04:46 2017 +0900 @@ -2,6 +2,7 @@ #include "hayatoBoomerang.h" #include "recieveController.h" #include "omni.h" +#include "gakugaku.h" Serial rs485(PC_10,PC_11,38400); @@ -9,11 +10,8 @@ PwmOut led(LED1); DigitalIn button(BUTTON1); boomerang boom(&rs485); -Thread commThread; +gakugaku fishingPole(&rs485); recieveController con(PC_12,PD_2,198); -// recieveController con(PA_0,PA_1,198); -// Serial con(PA_0,PA_1,115200); - void init() { @@ -25,37 +23,45 @@ boom.setTargetArmAngle(40); } -void updater() -{ - boom.update(); -} - int main() { init(); - int angle = 5,turnRate = 127; - bool prevAnglechange = false; + int angle = 10,turnRate = 127; + double angling; + bool prevAnglechange = false,prevSwing=false; char data[20],*data2,*data3; // commThread.start(&comm); // ticker.attach(&updater,0.01); while(true) { - if((con.buttons[0] >>7) ) + if((con.buttons[0] >>7)%2 ) boom.fire(); - if(con.buttons[0] >>8) - boom.setTargetMotorSpeed(8000); + + if((con.buttons[0] >>8)%2) + boom.setTargetMotorSpeed(13000); else boom.setTargetMotorSpeed(0); + if((con.buttons[0]>>6)%2) + angle = 10; + + if((con.buttons[0]>>13)%2 ) + { + if(!prevSwing) + fishingPole.toggleSwing(); + prevSwing = true; + }else{ + prevSwing = false; + } if(con.buttons[3] < 117 ) { if(!prevAnglechange) - angle += 1; + angle += 5; prevAnglechange =true; }else if(con.buttons[3] > 137) { if(!prevAnglechange) - angle -= 1; + angle -= 5; prevAnglechange =true; }else{ prevAnglechange =false; @@ -67,19 +73,32 @@ angle = 80; } - if((con.buttons[0] >> 9)) - turnRate = 227; - else if((con.buttons[0] >> 10)) - turnRate = 27; + if((con.buttons[0] >> 9)%2) + turnRate = 127+127*.2; //turn at .2 power + else if((con.buttons[0] >> 10)%2) + turnRate = 127-127*.2; else turnRate = 127; + if((con.buttons[0]>>14)%2) + angling = 0.4; + else if((con.buttons[0]>>12)%2) + angling = -0.4; + else + angling = 0; + fishingPole.setAnglingSpeed(angling); + + if((con.buttons[0]>>15)%2) + fishingPole.Deploy(); + + fishingPole.update(); + boom.setTargetArmAngle(angle); led = boom.beltSpeed; boom.move(con.buttons[1],con.buttons[2],turnRate); boom.update(); - printf("SPPED:%d\n",con.buttons[0]); + printf("PID input:%d PID result %d\n",angle,con.buttons[0]); wait_ms(20); /* code */ con.update();