WirelessComs For David
Dependencies: EthernetNetIf Queue Servo mbed
main.cpp@0:8101522bb770, 2013-01-29 (annotated)
- Committer:
- johnson6987
- Date:
- Tue Jan 29 18:24:44 2013 +0000
- Revision:
- 0:8101522bb770
- Child:
- 2:6383a282c70c
- Child:
- 3:9844ec849c12
For David
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
johnson6987 | 0:8101522bb770 | 1 | #include "mbed.h" |
johnson6987 | 0:8101522bb770 | 2 | #include "EthernetNetIf.h" |
johnson6987 | 0:8101522bb770 | 3 | #include "UDPSocket.h" |
johnson6987 | 0:8101522bb770 | 4 | #include "queue.h" |
johnson6987 | 0:8101522bb770 | 5 | #include "Servo.h" |
johnson6987 | 0:8101522bb770 | 6 | |
johnson6987 | 0:8101522bb770 | 7 | Ticker NetPoll; |
johnson6987 | 0:8101522bb770 | 8 | Timer ErrorTime; |
johnson6987 | 0:8101522bb770 | 9 | Queue UDP_queue(256,20); |
johnson6987 | 0:8101522bb770 | 10 | Serial pc(USBTX, USBRX); |
johnson6987 | 0:8101522bb770 | 11 | EthernetNetIf * eth; |
johnson6987 | 0:8101522bb770 | 12 | UDPSocket UDP; |
johnson6987 | 0:8101522bb770 | 13 | Host Robot; |
johnson6987 | 0:8101522bb770 | 14 | Servo ML(p21); |
johnson6987 | 0:8101522bb770 | 15 | Servo MR(p22); |
johnson6987 | 0:8101522bb770 | 16 | |
johnson6987 | 0:8101522bb770 | 17 | DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)}; |
johnson6987 | 0:8101522bb770 | 18 | |
johnson6987 | 0:8101522bb770 | 19 | char messageBufferIncoming[256]; |
johnson6987 | 0:8101522bb770 | 20 | |
johnson6987 | 0:8101522bb770 | 21 | struct Joystick { |
johnson6987 | 0:8101522bb770 | 22 | float axis[4]; |
johnson6987 | 0:8101522bb770 | 23 | float button[12]; |
johnson6987 | 0:8101522bb770 | 24 | }; |
johnson6987 | 0:8101522bb770 | 25 | |
johnson6987 | 0:8101522bb770 | 26 | |
johnson6987 | 0:8101522bb770 | 27 | Joystick Joy; |
johnson6987 | 0:8101522bb770 | 28 | void messageProcess(void) |
johnson6987 | 0:8101522bb770 | 29 | { |
johnson6987 | 0:8101522bb770 | 30 | |
johnson6987 | 0:8101522bb770 | 31 | ErrorTime.reset(); |
johnson6987 | 0:8101522bb770 | 32 | // pc.printf("%s\r\n",messageBufferIncoming); |
johnson6987 | 0:8101522bb770 | 33 | sscanf(messageBufferIncoming,"%f%f%f%f",\ |
johnson6987 | 0:8101522bb770 | 34 | &Joy.axis[0],\ |
johnson6987 | 0:8101522bb770 | 35 | &Joy.axis[1],\ |
johnson6987 | 0:8101522bb770 | 36 | &Joy.axis[2],\ |
johnson6987 | 0:8101522bb770 | 37 | &Joy.axis[3]); |
johnson6987 | 0:8101522bb770 | 38 | |
johnson6987 | 0:8101522bb770 | 39 | pc.printf("%f %f %f %f\r\n", Joy.axis[0],Joy.axis[1],Joy.axis[2],Joy.axis[3]); |
johnson6987 | 0:8101522bb770 | 40 | /* |
johnson6987 | 0:8101522bb770 | 41 | sscanf(messageBufferIncoming,"%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f",\ |
johnson6987 | 0:8101522bb770 | 42 | &Joy.axis[0],\ |
johnson6987 | 0:8101522bb770 | 43 | &Joy.axis[1],\ |
johnson6987 | 0:8101522bb770 | 44 | &Joy.axis[2],\ |
johnson6987 | 0:8101522bb770 | 45 | &Joy.axis[3],\ |
johnson6987 | 0:8101522bb770 | 46 | &Joy.button[0],\ |
johnson6987 | 0:8101522bb770 | 47 | &Joy.button[1],\ |
johnson6987 | 0:8101522bb770 | 48 | &Joy.button[2],\ |
johnson6987 | 0:8101522bb770 | 49 | &Joy.button[3],\ |
johnson6987 | 0:8101522bb770 | 50 | &Joy.button[4],\ |
johnson6987 | 0:8101522bb770 | 51 | &Joy.button[5],\ |
johnson6987 | 0:8101522bb770 | 52 | &Joy.button[6],\ |
johnson6987 | 0:8101522bb770 | 53 | &Joy.button[7],\ |
johnson6987 | 0:8101522bb770 | 54 | &Joy.button[8],\ |
johnson6987 | 0:8101522bb770 | 55 | &Joy.button[9],\ |
johnson6987 | 0:8101522bb770 | 56 | &Joy.button[10],\ |
johnson6987 | 0:8101522bb770 | 57 | &Joy.button[11]); |
johnson6987 | 0:8101522bb770 | 58 | |
johnson6987 | 0:8101522bb770 | 59 | */ |
johnson6987 | 0:8101522bb770 | 60 | |
johnson6987 | 0:8101522bb770 | 61 | /* |
johnson6987 | 0:8101522bb770 | 62 | float A=Joy.axis[1]; |
johnson6987 | 0:8101522bb770 | 63 | float B=Joy.axis[0]; |
johnson6987 | 0:8101522bb770 | 64 | if (A<.1 && B<.1 && B>-.1 && A>-.1) { |
johnson6987 | 0:8101522bb770 | 65 | MR=.5; |
johnson6987 | 0:8101522bb770 | 66 | ML=.5; |
johnson6987 | 0:8101522bb770 | 67 | } else { |
johnson6987 | 0:8101522bb770 | 68 | |
johnson6987 | 0:8101522bb770 | 69 | if (A>.1 || A<-.1) { |
johnson6987 | 0:8101522bb770 | 70 | ML=1-(A+1)/2; |
johnson6987 | 0:8101522bb770 | 71 | MR=(A+1)/2; |
johnson6987 | 0:8101522bb770 | 72 | } |
johnson6987 | 0:8101522bb770 | 73 | if (B>.25 || B<-.25) { |
johnson6987 | 0:8101522bb770 | 74 | ML=1-(B+1)/2; |
johnson6987 | 0:8101522bb770 | 75 | MR=1-(B+1)/2; |
johnson6987 | 0:8101522bb770 | 76 | } |
johnson6987 | 0:8101522bb770 | 77 | } |
johnson6987 | 0:8101522bb770 | 78 | |
johnson6987 | 0:8101522bb770 | 79 | */ |
johnson6987 | 0:8101522bb770 | 80 | ML=1; |
johnson6987 | 0:8101522bb770 | 81 | MR=0; |
johnson6987 | 0:8101522bb770 | 82 | for (int x=0; x<8; x++) { |
johnson6987 | 0:8101522bb770 | 83 | leds[x]=(bool)Joy.button[x]; |
johnson6987 | 0:8101522bb770 | 84 | } |
johnson6987 | 0:8101522bb770 | 85 | } |
johnson6987 | 0:8101522bb770 | 86 | |
johnson6987 | 0:8101522bb770 | 87 | void onUDPSocketEvent(UDPSocketEvent e) |
johnson6987 | 0:8101522bb770 | 88 | { |
johnson6987 | 0:8101522bb770 | 89 | switch (e) { |
johnson6987 | 0:8101522bb770 | 90 | case UDPSOCKET_READABLE: //The only event for now |
johnson6987 | 0:8101522bb770 | 91 | char buf[256] = {0}; |
johnson6987 | 0:8101522bb770 | 92 | Host host; |
johnson6987 | 0:8101522bb770 | 93 | while ( int len = UDP.recvfrom( buf, 255, &host ) ) { |
johnson6987 | 0:8101522bb770 | 94 | if ( len <= 0 ) |
johnson6987 | 0:8101522bb770 | 95 | break; |
johnson6987 | 0:8101522bb770 | 96 | UDP_queue.Put(buf); |
johnson6987 | 0:8101522bb770 | 97 | } |
johnson6987 | 0:8101522bb770 | 98 | break; |
johnson6987 | 0:8101522bb770 | 99 | } |
johnson6987 | 0:8101522bb770 | 100 | } |
johnson6987 | 0:8101522bb770 | 101 | |
johnson6987 | 0:8101522bb770 | 102 | |
johnson6987 | 0:8101522bb770 | 103 | int main() |
johnson6987 | 0:8101522bb770 | 104 | { |
johnson6987 | 0:8101522bb770 | 105 | |
johnson6987 | 0:8101522bb770 | 106 | eth = new EthernetNetIf( |
johnson6987 | 0:8101522bb770 | 107 | IpAddr(IpAddr(192,168,1,102)), // My IP Address |
johnson6987 | 0:8101522bb770 | 108 | IpAddr(IpAddr(255,255,255,0)), //Network Mask |
johnson6987 | 0:8101522bb770 | 109 | IpAddr(IpAddr(192,168,1,2)), //Gateway |
johnson6987 | 0:8101522bb770 | 110 | IpAddr(IpAddr()) //DNS |
johnson6987 | 0:8101522bb770 | 111 | ); |
johnson6987 | 0:8101522bb770 | 112 | Robot.setIp(IpAddr(192,168,1,102)); |
johnson6987 | 0:8101522bb770 | 113 | Robot.setPort(12345); |
johnson6987 | 0:8101522bb770 | 114 | |
johnson6987 | 0:8101522bb770 | 115 | eth->setup(); |
johnson6987 | 0:8101522bb770 | 116 | UDP.setOnEvent(&onUDPSocketEvent); |
johnson6987 | 0:8101522bb770 | 117 | UDP.bind(Robot); |
johnson6987 | 0:8101522bb770 | 118 | NetPoll.attach_us(&Net::poll,500); |
johnson6987 | 0:8101522bb770 | 119 | |
johnson6987 | 0:8101522bb770 | 120 | ErrorTime.start(); |
johnson6987 | 0:8101522bb770 | 121 | |
johnson6987 | 0:8101522bb770 | 122 | while (1) { |
johnson6987 | 0:8101522bb770 | 123 | if ( UDP_queue.Get(messageBufferIncoming))messageProcess(); |
johnson6987 | 0:8101522bb770 | 124 | if (ErrorTime.read()>.2) { |
johnson6987 | 0:8101522bb770 | 125 | MR=.50; |
johnson6987 | 0:8101522bb770 | 126 | ML=0.5; |
johnson6987 | 0:8101522bb770 | 127 | for (int x=0; x<8; x++)leds[x]=0; |
johnson6987 | 0:8101522bb770 | 128 | ErrorTime.reset(); |
johnson6987 | 0:8101522bb770 | 129 | } |
johnson6987 | 0:8101522bb770 | 130 | |
johnson6987 | 0:8101522bb770 | 131 | } |
johnson6987 | 0:8101522bb770 | 132 | } |
johnson6987 | 0:8101522bb770 | 133 |