2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
main.cpp@14:5cc44bec9cfc, 2017-10-05 (annotated)
- Committer:
- WAT34
- Date:
- Thu Oct 05 16:09:28 2017 +0900
- Revision:
- 14:5cc44bec9cfc
- Parent:
- 13:d01356a29b93
- Parent:
- 9:6540a10a07fb
- Child:
- 16:5f205692b14d
merged
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 | 6:0bee4b2bb400 | 23 | boom.setTargetArmAngle(40); |
WAT34 | 4:c2b09fa31492 | 24 | } |
WAT34 | 1:35e874e10602 | 25 | |
WAT34 | 1:35e874e10602 | 26 | int main() { |
WAT34 | 4:c2b09fa31492 | 27 | init(); |
WAT34 | 13:d01356a29b93 | 28 | int angle = 10,turnRate = 127; |
WAT34 | 13:d01356a29b93 | 29 | double angling; |
WAT34 | 13:d01356a29b93 | 30 | bool prevAnglechange = false,prevSwing=false; |
WAT34 | 5:9c52e6c14c87 | 31 | char data[20],*data2,*data3; |
WAT34 | 5:9c52e6c14c87 | 32 | // commThread.start(&comm); |
WAT34 | 6:0bee4b2bb400 | 33 | // ticker.attach(&updater,0.01); |
WAT34 | 4:c2b09fa31492 | 34 | while(true) |
WAT34 | 4:c2b09fa31492 | 35 | { |
WAT34 | 13:d01356a29b93 | 36 | if((con.buttons[0] >>7)%2 ) |
WAT34 | 6:0bee4b2bb400 | 37 | boom.fire(); |
WAT34 | 6:0bee4b2bb400 | 38 | |
WAT34 | 13:d01356a29b93 | 39 | if((con.buttons[0] >>8)%2) |
WAT34 | 13:d01356a29b93 | 40 | boom.setTargetMotorSpeed(13000); |
WAT34 | 6:0bee4b2bb400 | 41 | else |
WAT34 | 6:0bee4b2bb400 | 42 | boom.setTargetMotorSpeed(0); |
WAT34 | 6:0bee4b2bb400 | 43 | |
WAT34 | 13:d01356a29b93 | 44 | if((con.buttons[0]>>6)%2) |
WAT34 | 13:d01356a29b93 | 45 | angle = 10; |
WAT34 | 13:d01356a29b93 | 46 | |
WAT34 | 13:d01356a29b93 | 47 | if((con.buttons[0]>>13)%2 ) |
WAT34 | 13:d01356a29b93 | 48 | { |
WAT34 | 13:d01356a29b93 | 49 | if(!prevSwing) |
WAT34 | 13:d01356a29b93 | 50 | fishingPole.toggleSwing(); |
WAT34 | 13:d01356a29b93 | 51 | prevSwing = true; |
WAT34 | 13:d01356a29b93 | 52 | }else{ |
WAT34 | 13:d01356a29b93 | 53 | prevSwing = false; |
WAT34 | 13:d01356a29b93 | 54 | } |
WAT34 | 12:9d21189a8eb0 | 55 | if(con.buttons[3] < 117 ) |
WAT34 | 7:baf16d0c14e7 | 56 | { |
WAT34 | 7:baf16d0c14e7 | 57 | if(!prevAnglechange) |
WAT34 | 13:d01356a29b93 | 58 | angle += 5; |
WAT34 | 7:baf16d0c14e7 | 59 | prevAnglechange =true; |
WAT34 | 12:9d21189a8eb0 | 60 | }else |
WAT34 | 12:9d21189a8eb0 | 61 | if(con.buttons[3] > 137) |
WAT34 | 7:baf16d0c14e7 | 62 | { |
WAT34 | 7:baf16d0c14e7 | 63 | if(!prevAnglechange) |
WAT34 | 13:d01356a29b93 | 64 | angle -= 5; |
WAT34 | 7:baf16d0c14e7 | 65 | prevAnglechange =true; |
WAT34 | 7:baf16d0c14e7 | 66 | }else{ |
WAT34 | 7:baf16d0c14e7 | 67 | prevAnglechange =false; |
WAT34 | 7:baf16d0c14e7 | 68 | } |
WAT34 | 6:0bee4b2bb400 | 69 | if(angle < 5){ |
WAT34 | 6:0bee4b2bb400 | 70 | angle = 5; |
WAT34 | 6:0bee4b2bb400 | 71 | } |
WAT34 | 6:0bee4b2bb400 | 72 | if(angle > 80 ){ |
WAT34 | 6:0bee4b2bb400 | 73 | angle = 80; |
WAT34 | 6:0bee4b2bb400 | 74 | } |
WAT34 | 6:0bee4b2bb400 | 75 | |
WAT34 | 13:d01356a29b93 | 76 | if((con.buttons[0] >> 9)%2) |
WAT34 | 13:d01356a29b93 | 77 | turnRate = 127+127*.2; //turn at .2 power |
WAT34 | 13:d01356a29b93 | 78 | else if((con.buttons[0] >> 10)%2) |
WAT34 | 13:d01356a29b93 | 79 | turnRate = 127-127*.2; |
WAT34 | 12:9d21189a8eb0 | 80 | else |
WAT34 | 12:9d21189a8eb0 | 81 | turnRate = 127; |
WAT34 | 12:9d21189a8eb0 | 82 | |
WAT34 | 13:d01356a29b93 | 83 | if((con.buttons[0]>>14)%2) |
WAT34 | 13:d01356a29b93 | 84 | angling = 0.4; |
WAT34 | 13:d01356a29b93 | 85 | else if((con.buttons[0]>>12)%2) |
WAT34 | 13:d01356a29b93 | 86 | angling = -0.4; |
WAT34 | 13:d01356a29b93 | 87 | else |
WAT34 | 13:d01356a29b93 | 88 | angling = 0; |
WAT34 | 13:d01356a29b93 | 89 | fishingPole.setAnglingSpeed(angling); |
WAT34 | 13:d01356a29b93 | 90 | |
WAT34 | 13:d01356a29b93 | 91 | if((con.buttons[0]>>15)%2) |
WAT34 | 13:d01356a29b93 | 92 | fishingPole.Deploy(); |
WAT34 | 13:d01356a29b93 | 93 | |
WAT34 | 13:d01356a29b93 | 94 | fishingPole.update(); |
WAT34 | 13:d01356a29b93 | 95 | |
WAT34 | 12:9d21189a8eb0 | 96 | |
WAT34 | 6:0bee4b2bb400 | 97 | boom.setTargetArmAngle(angle); |
WAT34 | 6:0bee4b2bb400 | 98 | led = boom.beltSpeed; |
WAT34 | 12:9d21189a8eb0 | 99 | boom.move(con.buttons[1],con.buttons[2],turnRate); |
WAT34 | 4:c2b09fa31492 | 100 | boom.update(); |
WAT34 | 13:d01356a29b93 | 101 | printf("PID input:%d PID result %d\n",angle,con.buttons[0]); |
WAT34 | 6:0bee4b2bb400 | 102 | wait_ms(20); |
WAT34 | 5:9c52e6c14c87 | 103 | /* code */ |
WAT34 | 5:9c52e6c14c87 | 104 | con.update(); |
WAT34 | 5:9c52e6c14c87 | 105 | // printf("whats up\n" ); |
WAT34 | 4:c2b09fa31492 | 106 | |
WAT34 | 4:c2b09fa31492 | 107 | } |
WAT34 | 4:c2b09fa31492 | 108 | |
WAT34 | 1:35e874e10602 | 109 | } |