2017ロボコンはやとブーメランプログラム
Dependencies: PID QEI ikarashiMDC recieveController omni
main.cpp@12:9d21189a8eb0, 2017-09-13 (annotated)
- Committer:
- WAT34
- Date:
- Wed Sep 13 22:58:42 2017 +0700
- Revision:
- 12:9d21189a8eb0
- Parent:
- 11:68defd561031
- Child:
- 13:d01356a29b93
pinconfig fixed
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 | 6:0bee4b2bb400 | 5 | |
WAT34 | 1:35e874e10602 | 6 | |
WAT34 | 4:c2b09fa31492 | 7 | Serial rs485(PC_10,PC_11,38400); |
WAT34 | 5:9c52e6c14c87 | 8 | Serial serial(USBTX,USBRX); |
WAT34 | 4:c2b09fa31492 | 9 | PwmOut led(LED1); |
WAT34 | 4:c2b09fa31492 | 10 | DigitalIn button(BUTTON1); |
WAT34 | 4:c2b09fa31492 | 11 | boomerang boom(&rs485); |
WAT34 | 4:c2b09fa31492 | 12 | Thread commThread; |
WAT34 | 12:9d21189a8eb0 | 13 | recieveController con(PC_12,PD_2,198); |
WAT34 | 12:9d21189a8eb0 | 14 | // recieveController con(PA_0,PA_1,198); |
WAT34 | 5:9c52e6c14c87 | 15 | // Serial con(PA_0,PA_1,115200); |
WAT34 | 4:c2b09fa31492 | 16 | |
WAT34 | 4:c2b09fa31492 | 17 | |
WAT34 | 4:c2b09fa31492 | 18 | void init() |
WAT34 | 4:c2b09fa31492 | 19 | { |
WAT34 | 4:c2b09fa31492 | 20 | button.mode(PullUp); |
WAT34 | 4:c2b09fa31492 | 21 | |
WAT34 | 5:9c52e6c14c87 | 22 | serial.baud(115200); |
WAT34 | 4:c2b09fa31492 | 23 | //boom.calibrateArm(); |
WAT34 | 4:c2b09fa31492 | 24 | boom.setTargetMotorSpeed(0); |
WAT34 | 12:9d21189a8eb0 | 25 | boom.setTargetArmAngle(40); |
WAT34 | 5:9c52e6c14c87 | 26 | } |
WAT34 | 4:c2b09fa31492 | 27 | |
WAT34 | 12:9d21189a8eb0 | 28 | void updater() |
WAT34 | 12:9d21189a8eb0 | 29 | { |
WAT34 | 12:9d21189a8eb0 | 30 | boom.update(); |
WAT34 | 12:9d21189a8eb0 | 31 | } |
WAT34 | 1:35e874e10602 | 32 | |
WAT34 | 1:35e874e10602 | 33 | int main() { |
WAT34 | 4:c2b09fa31492 | 34 | init(); |
WAT34 | 12:9d21189a8eb0 | 35 | int angle = 5,turnRate = 127; |
WAT34 | 7:baf16d0c14e7 | 36 | bool prevAnglechange = false; |
WAT34 | 12:9d21189a8eb0 | 37 | char data[20],*data2,*data3; |
WAT34 | 5:9c52e6c14c87 | 38 | // commThread.start(&comm); |
WAT34 | 6:0bee4b2bb400 | 39 | // ticker.attach(&updater,0.01); |
WAT34 | 4:c2b09fa31492 | 40 | while(true) |
WAT34 | 4:c2b09fa31492 | 41 | { |
WAT34 | 12:9d21189a8eb0 | 42 | if((con.buttons[0] >>7) ) |
WAT34 | 6:0bee4b2bb400 | 43 | boom.fire(); |
WAT34 | 12:9d21189a8eb0 | 44 | if(con.buttons[0] >>8) |
WAT34 | 6:0bee4b2bb400 | 45 | boom.setTargetMotorSpeed(8000); |
WAT34 | 6:0bee4b2bb400 | 46 | else |
WAT34 | 6:0bee4b2bb400 | 47 | boom.setTargetMotorSpeed(0); |
WAT34 | 6:0bee4b2bb400 | 48 | |
WAT34 | 12:9d21189a8eb0 | 49 | if(con.buttons[3] < 117 ) |
WAT34 | 7:baf16d0c14e7 | 50 | { |
WAT34 | 7:baf16d0c14e7 | 51 | if(!prevAnglechange) |
WAT34 | 12:9d21189a8eb0 | 52 | angle += 1; |
WAT34 | 7:baf16d0c14e7 | 53 | prevAnglechange =true; |
WAT34 | 12:9d21189a8eb0 | 54 | }else |
WAT34 | 12:9d21189a8eb0 | 55 | if(con.buttons[3] > 137) |
WAT34 | 7:baf16d0c14e7 | 56 | { |
WAT34 | 7:baf16d0c14e7 | 57 | if(!prevAnglechange) |
WAT34 | 12:9d21189a8eb0 | 58 | angle -= 1; |
WAT34 | 7:baf16d0c14e7 | 59 | prevAnglechange =true; |
WAT34 | 7:baf16d0c14e7 | 60 | }else{ |
WAT34 | 7:baf16d0c14e7 | 61 | prevAnglechange =false; |
WAT34 | 7:baf16d0c14e7 | 62 | } |
WAT34 | 6:0bee4b2bb400 | 63 | if(angle < 5){ |
WAT34 | 6:0bee4b2bb400 | 64 | angle = 5; |
WAT34 | 6:0bee4b2bb400 | 65 | } |
WAT34 | 6:0bee4b2bb400 | 66 | if(angle > 80 ){ |
WAT34 | 6:0bee4b2bb400 | 67 | angle = 80; |
WAT34 | 6:0bee4b2bb400 | 68 | } |
WAT34 | 6:0bee4b2bb400 | 69 | |
WAT34 | 12:9d21189a8eb0 | 70 | if((con.buttons[0] >> 9)) |
WAT34 | 12:9d21189a8eb0 | 71 | turnRate = 227; |
WAT34 | 12:9d21189a8eb0 | 72 | else if((con.buttons[0] >> 10)) |
WAT34 | 12:9d21189a8eb0 | 73 | turnRate = 27; |
WAT34 | 12:9d21189a8eb0 | 74 | else |
WAT34 | 12:9d21189a8eb0 | 75 | turnRate = 127; |
WAT34 | 12:9d21189a8eb0 | 76 | |
WAT34 | 12:9d21189a8eb0 | 77 | |
WAT34 | 12:9d21189a8eb0 | 78 | boom.setTargetArmAngle(angle); |
WAT34 | 6:0bee4b2bb400 | 79 | led = boom.beltSpeed; |
WAT34 | 12:9d21189a8eb0 | 80 | boom.move(con.buttons[1],con.buttons[2],turnRate); |
WAT34 | 4:c2b09fa31492 | 81 | boom.update(); |
WAT34 | 12:9d21189a8eb0 | 82 | printf("SPPED:%d\n",con.buttons[0]); |
WAT34 | 6:0bee4b2bb400 | 83 | wait_ms(20); |
WAT34 | 5:9c52e6c14c87 | 84 | /* code */ |
WAT34 | 5:9c52e6c14c87 | 85 | con.update(); |
WAT34 | 5:9c52e6c14c87 | 86 | // printf("whats up\n" ); |
WAT34 | 4:c2b09fa31492 | 87 | |
WAT34 | 4:c2b09fa31492 | 88 | } |
WAT34 | 4:c2b09fa31492 | 89 | |
WAT34 | 1:35e874e10602 | 90 | } |