IR control car "spinner" 赤外線コントロールラジコン"spinner"の本体&プロポ統合版プログラムです
Dependencies: Motor Propo_RemotoIR mbed
spinner.cpp@0:6825e25b7428, 2013-10-16 (annotated)
- Committer:
- suupen
- Date:
- Wed Oct 16 02:45:27 2013 +0000
- Revision:
- 0:6825e25b7428
IR control car "spinner"
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
suupen | 0:6825e25b7428 | 1 | /* spinner base pcb |
suupen | 0:6825e25b7428 | 2 | * <use ir propo> |
suupen | 0:6825e25b7428 | 3 | * LPC1114FN28 sony format propo |
suupen | 0:6825e25b7428 | 4 | * |
suupen | 0:6825e25b7428 | 5 | * <spinner circit> |
suupen | 0:6825e25b7428 | 6 | * LPC1114FN28 |
suupen | 0:6825e25b7428 | 7 | * dp28 : ir reciver |
suupen | 0:6825e25b7428 | 8 | * dp18,dp17,dp25: (pwm fwd rev) Right motor control |
suupen | 0:6825e25b7428 | 9 | * dp2,dp4,dp6:(pwm fwd rev) Left motor control |
suupen | 0:6825e25b7428 | 10 | * |
suupen | 0:6825e25b7428 | 11 | * dp11,dp13,dp14,dp26 (LED1,2,3,4):lookup the ir recive condition |
suupen | 0:6825e25b7428 | 12 | |
suupen | 0:6825e25b7428 | 13 | */ |
suupen | 0:6825e25b7428 | 14 | |
suupen | 0:6825e25b7428 | 15 | #include "mbed.h" |
suupen | 0:6825e25b7428 | 16 | #include "Motor.h" |
suupen | 0:6825e25b7428 | 17 | |
suupen | 0:6825e25b7428 | 18 | #include "ReceiverIR.h" |
suupen | 0:6825e25b7428 | 19 | |
suupen | 0:6825e25b7428 | 20 | #define DBG |
suupen | 0:6825e25b7428 | 21 | //#define DBG2 // motor dousa kakunin |
suupen | 0:6825e25b7428 | 22 | |
suupen | 0:6825e25b7428 | 23 | #ifdef DBG |
suupen | 0:6825e25b7428 | 24 | Serial pc(dp16, dp15); //tx,rx |
suupen | 0:6825e25b7428 | 25 | #endif // DBG |
suupen | 0:6825e25b7428 | 26 | |
suupen | 0:6825e25b7428 | 27 | //----------------------- |
suupen | 0:6825e25b7428 | 28 | // ir Library |
suupen | 0:6825e25b7428 | 29 | ReceiverIR ir_rx(dp28); |
suupen | 0:6825e25b7428 | 30 | RemoteIR::Format formatRx; |
suupen | 0:6825e25b7428 | 31 | uint8_t bufRx[10]; // nomal no is "2". |
suupen | 0:6825e25b7428 | 32 | int bitcountRx; |
suupen | 0:6825e25b7428 | 33 | |
suupen | 0:6825e25b7428 | 34 | DigitalOut led1(dp11); |
suupen | 0:6825e25b7428 | 35 | DigitalOut led2(dp13); |
suupen | 0:6825e25b7428 | 36 | DigitalOut led3(dp14); |
suupen | 0:6825e25b7428 | 37 | DigitalOut led4(dp26); |
suupen | 0:6825e25b7428 | 38 | |
suupen | 0:6825e25b7428 | 39 | |
suupen | 0:6825e25b7428 | 40 | //----------------------- |
suupen | 0:6825e25b7428 | 41 | // Motor Library |
suupen | 0:6825e25b7428 | 42 | Motor Right(dp18, dp17, dp25); // pwm,fwd,rev |
suupen | 0:6825e25b7428 | 43 | Motor Left(dp2, dp4, dp6); // pwm,fwd,rev |
suupen | 0:6825e25b7428 | 44 | //----------------------- |
suupen | 0:6825e25b7428 | 45 | |
suupen | 0:6825e25b7428 | 46 | Timeout timeout; |
suupen | 0:6825e25b7428 | 47 | |
suupen | 0:6825e25b7428 | 48 | void isr_timeout(void) |
suupen | 0:6825e25b7428 | 49 | { |
suupen | 0:6825e25b7428 | 50 | Right.speed(0); |
suupen | 0:6825e25b7428 | 51 | Left.speed(0); |
suupen | 0:6825e25b7428 | 52 | |
suupen | 0:6825e25b7428 | 53 | timeout.detach(); |
suupen | 0:6825e25b7428 | 54 | timeout.attach_us(&isr_timeout, 1000 * 1000); |
suupen | 0:6825e25b7428 | 55 | } |
suupen | 0:6825e25b7428 | 56 | |
suupen | 0:6825e25b7428 | 57 | void initialSpinner(void){ |
suupen | 0:6825e25b7428 | 58 | |
suupen | 0:6825e25b7428 | 59 | timeout.detach(); |
suupen | 0:6825e25b7428 | 60 | timeout.attach_us(&isr_timeout, 1000 * 1000); |
suupen | 0:6825e25b7428 | 61 | |
suupen | 0:6825e25b7428 | 62 | } |
suupen | 0:6825e25b7428 | 63 | |
suupen | 0:6825e25b7428 | 64 | void mainSpinner(void) |
suupen | 0:6825e25b7428 | 65 | { |
suupen | 0:6825e25b7428 | 66 | #ifdef DBG |
suupen | 0:6825e25b7428 | 67 | pc.baud(9600); |
suupen | 0:6825e25b7428 | 68 | #endif // DBG |
suupen | 0:6825e25b7428 | 69 | |
suupen | 0:6825e25b7428 | 70 | // IR data Monitor |
suupen | 0:6825e25b7428 | 71 | led1 = 0; |
suupen | 0:6825e25b7428 | 72 | led2 = 0; |
suupen | 0:6825e25b7428 | 73 | led3 = 0; |
suupen | 0:6825e25b7428 | 74 | led4 = 0; |
suupen | 0:6825e25b7428 | 75 | |
suupen | 0:6825e25b7428 | 76 | switch(ir_rx.getState()) { |
suupen | 0:6825e25b7428 | 77 | case ReceiverIR::Idle: |
suupen | 0:6825e25b7428 | 78 | led1 = 1; |
suupen | 0:6825e25b7428 | 79 | break; |
suupen | 0:6825e25b7428 | 80 | case ReceiverIR::Receiving: |
suupen | 0:6825e25b7428 | 81 | led2 = 1; |
suupen | 0:6825e25b7428 | 82 | break; |
suupen | 0:6825e25b7428 | 83 | case ReceiverIR::Received: |
suupen | 0:6825e25b7428 | 84 | led3 = 1; |
suupen | 0:6825e25b7428 | 85 | break; |
suupen | 0:6825e25b7428 | 86 | default: |
suupen | 0:6825e25b7428 | 87 | led4 = 1; |
suupen | 0:6825e25b7428 | 88 | break; |
suupen | 0:6825e25b7428 | 89 | } |
suupen | 0:6825e25b7428 | 90 | |
suupen | 0:6825e25b7428 | 91 | if (ir_rx.getState() == ReceiverIR::Received) { |
suupen | 0:6825e25b7428 | 92 | bitcountRx = ir_rx.getData(&formatRx, bufRx, sizeof(bufRx) * 8); |
suupen | 0:6825e25b7428 | 93 | |
suupen | 0:6825e25b7428 | 94 | if((formatRx == RemoteIR::SONY) && (bitcountRx == 16)) { |
suupen | 0:6825e25b7428 | 95 | |
suupen | 0:6825e25b7428 | 96 | timeout.detach(); |
suupen | 0:6825e25b7428 | 97 | timeout.attach_us(&isr_timeout, 1000 * 1000); |
suupen | 0:6825e25b7428 | 98 | |
suupen | 0:6825e25b7428 | 99 | float xWork = (float)((int8_t)bufRx[0] / 8.0f); |
suupen | 0:6825e25b7428 | 100 | float yWork = (float)((int8_t)bufRx[1] / 8.0f); |
suupen | 0:6825e25b7428 | 101 | |
suupen | 0:6825e25b7428 | 102 | // fukantai (-0.2 - 0.2) |
suupen | 0:6825e25b7428 | 103 | if((-0.2 <= xWork) && (xWork <= 0.2)){xWork = 0;} |
suupen | 0:6825e25b7428 | 104 | if((-0.2 <= yWork) && (yWork <= 0.2)){yWork = 0;} |
suupen | 0:6825e25b7428 | 105 | |
suupen | 0:6825e25b7428 | 106 | yWork = -yWork; // sousa wo hanten. |
suupen | 0:6825e25b7428 | 107 | |
suupen | 0:6825e25b7428 | 108 | float r = yWork + (xWork / 2.0); |
suupen | 0:6825e25b7428 | 109 | float l = yWork - (xWork / 2.0); |
suupen | 0:6825e25b7428 | 110 | |
suupen | 0:6825e25b7428 | 111 | if(-1.0 > r) { |
suupen | 0:6825e25b7428 | 112 | r = -1.0; |
suupen | 0:6825e25b7428 | 113 | } |
suupen | 0:6825e25b7428 | 114 | if( 1.0 < r) { |
suupen | 0:6825e25b7428 | 115 | r = 1.0; |
suupen | 0:6825e25b7428 | 116 | } |
suupen | 0:6825e25b7428 | 117 | if(-1.0 > l) { |
suupen | 0:6825e25b7428 | 118 | l = -1.0; |
suupen | 0:6825e25b7428 | 119 | } |
suupen | 0:6825e25b7428 | 120 | if( 1.0 < l) { |
suupen | 0:6825e25b7428 | 121 | l = 1.0; |
suupen | 0:6825e25b7428 | 122 | } |
suupen | 0:6825e25b7428 | 123 | |
suupen | 0:6825e25b7428 | 124 | |
suupen | 0:6825e25b7428 | 125 | #ifdef DBG |
suupen | 0:6825e25b7428 | 126 | pc.printf("x = %02d y = %02d |xWork = %1.1f yWork = %1.1f |r = %1.1f l = %1.1f \r\n",(int8_t)bufRx[0],(int8_t)bufRx[1],xWork,yWork,r,l); |
suupen | 0:6825e25b7428 | 127 | #endif // DBG |
suupen | 0:6825e25b7428 | 128 | |
suupen | 0:6825e25b7428 | 129 | Right.speed(r); |
suupen | 0:6825e25b7428 | 130 | Left.speed(-l); |
suupen | 0:6825e25b7428 | 131 | |
suupen | 0:6825e25b7428 | 132 | |
suupen | 0:6825e25b7428 | 133 | |
suupen | 0:6825e25b7428 | 134 | } |
suupen | 0:6825e25b7428 | 135 | } |
suupen | 0:6825e25b7428 | 136 | } |
suupen | 0:6825e25b7428 | 137 |