2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
main.cpp@16:5f205692b14d, 2017-10-06 (annotated)
- Committer:
- WAT34
- Date:
- Fri Oct 06 21:59:16 2017 +0900
- Revision:
- 16:5f205692b14d
- Parent:
- 14:5cc44bec9cfc
- Child:
- 17:311aed3cad15
need to fix bug
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WAT34 | 1:35e874e10602 | 1 | #include "mbed.h" |
WAT34 | 1:35e874e10602 | 2 | #include "hayatoBoomerang.h" |
WAT34 | 5:9c52e6c14c87 | 3 | #include "recieveController.h" |
WAT34 | 6:0bee4b2bb400 | 4 | #include "omni.h" |
WAT34 | 13:d01356a29b93 | 5 | #include "gakugaku.h" |
WAT34 | 6:0bee4b2bb400 | 6 | |
WAT34 | 1:35e874e10602 | 7 | |
WAT34 | 4:c2b09fa31492 | 8 | Serial rs485(PC_10,PC_11,38400); |
WAT34 | 5:9c52e6c14c87 | 9 | Serial serial(USBTX,USBRX); |
WAT34 | 4:c2b09fa31492 | 10 | PwmOut led(LED1); |
WAT34 | 4:c2b09fa31492 | 11 | DigitalIn button(BUTTON1); |
WAT34 | 4:c2b09fa31492 | 12 | boomerang boom(&rs485); |
WAT34 | 13:d01356a29b93 | 13 | gakugaku fishingPole(&rs485); |
eil4nyqn | 8:c47cf4e0230c | 14 | recieveController con(PC_12,PD_2,198); |
WAT34 | 4:c2b09fa31492 | 15 | |
WAT34 | 4:c2b09fa31492 | 16 | void init() |
WAT34 | 4:c2b09fa31492 | 17 | { |
WAT34 | 4:c2b09fa31492 | 18 | button.mode(PullUp); |
WAT34 | 4:c2b09fa31492 | 19 | |
WAT34 | 5:9c52e6c14c87 | 20 | serial.baud(115200); |
WAT34 | 4:c2b09fa31492 | 21 | //boom.calibrateArm(); |
WAT34 | 4:c2b09fa31492 | 22 | boom.setTargetMotorSpeed(0); |
WAT34 | 4:c2b09fa31492 | 23 | } |
WAT34 | 1:35e874e10602 | 24 | |
WAT34 | 1:35e874e10602 | 25 | int main() { |
WAT34 | 4:c2b09fa31492 | 26 | init(); |
WAT34 | 13:d01356a29b93 | 27 | int angle = 10,turnRate = 127; |
WAT34 | 13:d01356a29b93 | 28 | double angling; |
WAT34 | 13:d01356a29b93 | 29 | bool prevAnglechange = false,prevSwing=false; |
WAT34 | 5:9c52e6c14c87 | 30 | char data[20],*data2,*data3; |
WAT34 | 5:9c52e6c14c87 | 31 | // commThread.start(&comm); |
WAT34 | 6:0bee4b2bb400 | 32 | // ticker.attach(&updater,0.01); |
WAT34 | 4:c2b09fa31492 | 33 | while(true) |
WAT34 | 4:c2b09fa31492 | 34 | { |
WAT34 | 13:d01356a29b93 | 35 | if((con.buttons[0] >>7)%2 ) |
WAT34 | 6:0bee4b2bb400 | 36 | boom.fire(); |
WAT34 | 6:0bee4b2bb400 | 37 | |
WAT34 | 13:d01356a29b93 | 38 | if((con.buttons[0] >>8)%2) |
WAT34 | 13:d01356a29b93 | 39 | boom.setTargetMotorSpeed(13000); |
WAT34 | 6:0bee4b2bb400 | 40 | else |
WAT34 | 6:0bee4b2bb400 | 41 | boom.setTargetMotorSpeed(0); |
WAT34 | 6:0bee4b2bb400 | 42 | |
WAT34 | 16:5f205692b14d | 43 | |
WAT34 | 16:5f205692b14d | 44 | if((con.buttons[0]>>3)%2) //square |
WAT34 | 16:5f205692b14d | 45 | angle = 80; |
WAT34 | 16:5f205692b14d | 46 | if((con.buttons[0]>>4)%2) //cross |
WAT34 | 16:5f205692b14d | 47 | angle = 45; |
WAT34 | 16:5f205692b14d | 48 | if((con.buttons[0]>>5)%2) //circle |
WAT34 | 16:5f205692b14d | 49 | angle = 25; |
WAT34 | 16:5f205692b14d | 50 | if((con.buttons[0]>>6)%2) //triangle |
WAT34 | 13:d01356a29b93 | 51 | angle = 10; |
WAT34 | 13:d01356a29b93 | 52 | if((con.buttons[0]>>13)%2 ) |
WAT34 | 13:d01356a29b93 | 53 | { |
WAT34 | 13:d01356a29b93 | 54 | if(!prevSwing) |
WAT34 | 13:d01356a29b93 | 55 | fishingPole.toggleSwing(); |
WAT34 | 13:d01356a29b93 | 56 | prevSwing = true; |
WAT34 | 13:d01356a29b93 | 57 | }else{ |
WAT34 | 13:d01356a29b93 | 58 | prevSwing = false; |
WAT34 | 13:d01356a29b93 | 59 | } |
WAT34 | 12:9d21189a8eb0 | 60 | if(con.buttons[3] < 117 ) |
WAT34 | 7:baf16d0c14e7 | 61 | { |
WAT34 | 7:baf16d0c14e7 | 62 | if(!prevAnglechange) |
WAT34 | 13:d01356a29b93 | 63 | angle += 5; |
WAT34 | 7:baf16d0c14e7 | 64 | prevAnglechange =true; |
WAT34 | 12:9d21189a8eb0 | 65 | }else |
WAT34 | 12:9d21189a8eb0 | 66 | if(con.buttons[3] > 137) |
WAT34 | 7:baf16d0c14e7 | 67 | { |
WAT34 | 7:baf16d0c14e7 | 68 | if(!prevAnglechange) |
WAT34 | 13:d01356a29b93 | 69 | angle -= 5; |
WAT34 | 7:baf16d0c14e7 | 70 | prevAnglechange =true; |
WAT34 | 7:baf16d0c14e7 | 71 | }else{ |
WAT34 | 7:baf16d0c14e7 | 72 | prevAnglechange =false; |
WAT34 | 7:baf16d0c14e7 | 73 | } |
WAT34 | 6:0bee4b2bb400 | 74 | if(angle < 5){ |
WAT34 | 6:0bee4b2bb400 | 75 | angle = 5; |
WAT34 | 6:0bee4b2bb400 | 76 | } |
WAT34 | 6:0bee4b2bb400 | 77 | if(angle > 80 ){ |
WAT34 | 6:0bee4b2bb400 | 78 | angle = 80; |
WAT34 | 6:0bee4b2bb400 | 79 | } |
WAT34 | 6:0bee4b2bb400 | 80 | |
WAT34 | 13:d01356a29b93 | 81 | if((con.buttons[0] >> 9)%2) |
WAT34 | 13:d01356a29b93 | 82 | turnRate = 127+127*.2; //turn at .2 power |
WAT34 | 13:d01356a29b93 | 83 | else if((con.buttons[0] >> 10)%2) |
WAT34 | 13:d01356a29b93 | 84 | turnRate = 127-127*.2; |
WAT34 | 12:9d21189a8eb0 | 85 | else |
WAT34 | 12:9d21189a8eb0 | 86 | turnRate = 127; |
WAT34 | 12:9d21189a8eb0 | 87 | |
WAT34 | 13:d01356a29b93 | 88 | if((con.buttons[0]>>14)%2) |
WAT34 | 13:d01356a29b93 | 89 | angling = 0.4; |
WAT34 | 13:d01356a29b93 | 90 | else if((con.buttons[0]>>12)%2) |
WAT34 | 13:d01356a29b93 | 91 | angling = -0.4; |
WAT34 | 13:d01356a29b93 | 92 | else |
WAT34 | 13:d01356a29b93 | 93 | angling = 0; |
WAT34 | 13:d01356a29b93 | 94 | fishingPole.setAnglingSpeed(angling); |
WAT34 | 13:d01356a29b93 | 95 | |
WAT34 | 13:d01356a29b93 | 96 | if((con.buttons[0]>>15)%2) |
WAT34 | 13:d01356a29b93 | 97 | fishingPole.Deploy(); |
WAT34 | 13:d01356a29b93 | 98 | |
WAT34 | 16:5f205692b14d | 99 | |
WAT34 | 13:d01356a29b93 | 100 | fishingPole.update(); |
WAT34 | 13:d01356a29b93 | 101 | |
WAT34 | 12:9d21189a8eb0 | 102 | |
WAT34 | 6:0bee4b2bb400 | 103 | boom.setTargetArmAngle(angle); |
WAT34 | 6:0bee4b2bb400 | 104 | led = boom.beltSpeed; |
WAT34 | 12:9d21189a8eb0 | 105 | boom.move(con.buttons[1],con.buttons[2],turnRate); |
WAT34 | 4:c2b09fa31492 | 106 | boom.update(); |
WAT34 | 16:5f205692b14d | 107 | |
WAT34 | 16:5f205692b14d | 108 | serial.printf("controller:%d",con.buttons[0]); |
WAT34 | 6:0bee4b2bb400 | 109 | wait_ms(20); |
WAT34 | 5:9c52e6c14c87 | 110 | con.update(); |
WAT34 | 4:c2b09fa31492 | 111 | |
WAT34 | 4:c2b09fa31492 | 112 | } |
WAT34 | 4:c2b09fa31492 | 113 | |
WAT34 | 1:35e874e10602 | 114 | } |