WirelessComs For David
Dependencies: EthernetNetIf Queue Servo mbed
main.cpp@4:bae1c47dc112, 2013-07-29 (annotated)
- Committer:
- johnson6987
- Date:
- Mon Jul 29 20:08:37 2013 +0000
- Revision:
- 4:bae1c47dc112
- Parent:
- 2:6383a282c70c
Wireless Communication for Protomicro
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 | 2:6383a282c70c | 14 | Servo Ml(p21); |
johnson6987 | 2:6383a282c70c | 15 | Servo Mr(p22); |
johnson6987 | 2:6383a282c70c | 16 | Servo Arm(p23); |
johnson6987 | 2:6383a282c70c | 17 | Servo Elbow(p24); |
johnson6987 | 2:6383a282c70c | 18 | float MR=.5; |
johnson6987 | 2:6383a282c70c | 19 | float ML=.5; |
johnson6987 | 0:8101522bb770 | 20 | |
johnson6987 | 0:8101522bb770 | 21 | DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)}; |
johnson6987 | 0:8101522bb770 | 22 | |
johnson6987 | 0:8101522bb770 | 23 | char messageBufferIncoming[256]; |
johnson6987 | 0:8101522bb770 | 24 | |
johnson6987 | 0:8101522bb770 | 25 | struct Joystick { |
johnson6987 | 0:8101522bb770 | 26 | float axis[4]; |
johnson6987 | 0:8101522bb770 | 27 | float button[12]; |
johnson6987 | 0:8101522bb770 | 28 | }; |
johnson6987 | 0:8101522bb770 | 29 | |
johnson6987 | 2:6383a282c70c | 30 | void right(){ |
johnson6987 | 2:6383a282c70c | 31 | Ml=1; |
johnson6987 | 2:6383a282c70c | 32 | Mr=1; |
johnson6987 | 2:6383a282c70c | 33 | wait(.25); |
johnson6987 | 2:6383a282c70c | 34 | } |
johnson6987 | 2:6383a282c70c | 35 | |
johnson6987 | 2:6383a282c70c | 36 | void left(){ |
johnson6987 | 2:6383a282c70c | 37 | Ml=0; |
johnson6987 | 2:6383a282c70c | 38 | Mr=0; |
johnson6987 | 2:6383a282c70c | 39 | wait(.25); |
johnson6987 | 2:6383a282c70c | 40 | } |
johnson6987 | 2:6383a282c70c | 41 | |
johnson6987 | 2:6383a282c70c | 42 | void forward(float time){ |
johnson6987 | 2:6383a282c70c | 43 | Ml=1; |
johnson6987 | 2:6383a282c70c | 44 | Mr=0; |
johnson6987 | 2:6383a282c70c | 45 | wait(time); |
johnson6987 | 2:6383a282c70c | 46 | } |
johnson6987 | 2:6383a282c70c | 47 | |
johnson6987 | 2:6383a282c70c | 48 | void backwards(float time){ |
johnson6987 | 2:6383a282c70c | 49 | Ml=0; |
johnson6987 | 2:6383a282c70c | 50 | Mr=1; |
johnson6987 | 2:6383a282c70c | 51 | wait(time); |
johnson6987 | 2:6383a282c70c | 52 | } |
johnson6987 | 2:6383a282c70c | 53 | |
johnson6987 | 0:8101522bb770 | 54 | |
johnson6987 | 0:8101522bb770 | 55 | Joystick Joy; |
johnson6987 | 0:8101522bb770 | 56 | void messageProcess(void) |
johnson6987 | 0:8101522bb770 | 57 | { |
johnson6987 | 0:8101522bb770 | 58 | |
johnson6987 | 0:8101522bb770 | 59 | ErrorTime.reset(); |
johnson6987 | 0:8101522bb770 | 60 | // pc.printf("%s\r\n",messageBufferIncoming); |
johnson6987 | 0:8101522bb770 | 61 | sscanf(messageBufferIncoming,"%f%f%f%f",\ |
johnson6987 | 0:8101522bb770 | 62 | &Joy.axis[0],\ |
johnson6987 | 0:8101522bb770 | 63 | &Joy.axis[1],\ |
johnson6987 | 0:8101522bb770 | 64 | &Joy.axis[2],\ |
johnson6987 | 0:8101522bb770 | 65 | &Joy.axis[3]); |
johnson6987 | 0:8101522bb770 | 66 | |
johnson6987 | 2:6383a282c70c | 67 | // pc.printf("%f %f %f %f\r\n", Joy.axis[0],Joy.axis[1],Joy.axis[2],Joy.axis[3]); |
johnson6987 | 0:8101522bb770 | 68 | /* |
johnson6987 | 0:8101522bb770 | 69 | sscanf(messageBufferIncoming,"%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f",\ |
johnson6987 | 0:8101522bb770 | 70 | &Joy.axis[0],\ |
johnson6987 | 0:8101522bb770 | 71 | &Joy.axis[1],\ |
johnson6987 | 0:8101522bb770 | 72 | &Joy.axis[2],\ |
johnson6987 | 0:8101522bb770 | 73 | &Joy.axis[3],\ |
johnson6987 | 0:8101522bb770 | 74 | &Joy.button[0],\ |
johnson6987 | 0:8101522bb770 | 75 | &Joy.button[1],\ |
johnson6987 | 0:8101522bb770 | 76 | &Joy.button[2],\ |
johnson6987 | 0:8101522bb770 | 77 | &Joy.button[3],\ |
johnson6987 | 0:8101522bb770 | 78 | &Joy.button[4],\ |
johnson6987 | 0:8101522bb770 | 79 | &Joy.button[5],\ |
johnson6987 | 0:8101522bb770 | 80 | &Joy.button[6],\ |
johnson6987 | 0:8101522bb770 | 81 | &Joy.button[7],\ |
johnson6987 | 0:8101522bb770 | 82 | &Joy.button[8],\ |
johnson6987 | 0:8101522bb770 | 83 | &Joy.button[9],\ |
johnson6987 | 0:8101522bb770 | 84 | &Joy.button[10],\ |
johnson6987 | 0:8101522bb770 | 85 | &Joy.button[11]); |
johnson6987 | 0:8101522bb770 | 86 | |
johnson6987 | 0:8101522bb770 | 87 | */ |
johnson6987 | 0:8101522bb770 | 88 | |
johnson6987 | 2:6383a282c70c | 89 | |
johnson6987 | 2:6383a282c70c | 90 | float Y=Joy.axis[1]; |
johnson6987 | 2:6383a282c70c | 91 | float X=Joy.axis[0]; |
johnson6987 | 2:6383a282c70c | 92 | Arm=Joy.axis[2]; |
johnson6987 | 2:6383a282c70c | 93 | Elbow=(Joy.axis[3]+1)/2.0; |
johnson6987 | 2:6383a282c70c | 94 | Y=-Y; |
johnson6987 | 2:6383a282c70c | 95 | |
johnson6987 | 2:6383a282c70c | 96 | |
johnson6987 | 2:6383a282c70c | 97 | if (X<.1 && Y<.1 && X>-.1 && Y>-.1) { |
johnson6987 | 2:6383a282c70c | 98 | Ml=.5; |
johnson6987 | 2:6383a282c70c | 99 | Mr=.5; |
johnson6987 | 0:8101522bb770 | 100 | } else { |
johnson6987 | 2:6383a282c70c | 101 | |
johnson6987 | 2:6383a282c70c | 102 | if (Y>0 && X>0) { |
johnson6987 | 2:6383a282c70c | 103 | ML=sqrt((X*X)+(Y*Y)); |
johnson6987 | 2:6383a282c70c | 104 | MR=Y-X; |
johnson6987 | 2:6383a282c70c | 105 | } |
johnson6987 | 2:6383a282c70c | 106 | if (Y>0 && X<0) { |
johnson6987 | 2:6383a282c70c | 107 | ML=Y+X; |
johnson6987 | 2:6383a282c70c | 108 | MR=sqrt((X*X)+(Y*Y)); |
johnson6987 | 0:8101522bb770 | 109 | } |
johnson6987 | 2:6383a282c70c | 110 | if (Y<0 && X<0) { |
johnson6987 | 2:6383a282c70c | 111 | ML=Y-X; |
johnson6987 | 2:6383a282c70c | 112 | MR=-sqrt((X*X)+(Y*Y)); |
johnson6987 | 2:6383a282c70c | 113 | } |
johnson6987 | 2:6383a282c70c | 114 | if (Y<0 && X>0) { |
johnson6987 | 2:6383a282c70c | 115 | ML=-sqrt((X*X)+(Y*Y)); |
johnson6987 | 2:6383a282c70c | 116 | MR=Y+X; |
johnson6987 | 0:8101522bb770 | 117 | } |
johnson6987 | 2:6383a282c70c | 118 | Ml=(ML+1)/2.0; |
johnson6987 | 2:6383a282c70c | 119 | Mr=(1-MR)/2.0; |
johnson6987 | 0:8101522bb770 | 120 | } |
johnson6987 | 2:6383a282c70c | 121 | pc.printf("X=%f Y=%f ML=%f MR=%f\r\n", X,Y,Ml.read(),Mr.read()); |
johnson6987 | 2:6383a282c70c | 122 | /* |
johnson6987 | 0:8101522bb770 | 123 | |
johnson6987 | 4:bae1c47dc112 | 124 | ML=1; |
johnson6987 | 0:8101522bb770 | 125 | MR=0; |
johnson6987 | 0:8101522bb770 | 126 | for (int x=0; x<8; x++) { |
johnson6987 | 0:8101522bb770 | 127 | leds[x]=(bool)Joy.button[x]; |
johnson6987 | 2:6383a282c70c | 128 | }*/ |
johnson6987 | 0:8101522bb770 | 129 | } |
johnson6987 | 0:8101522bb770 | 130 | |
johnson6987 | 0:8101522bb770 | 131 | void onUDPSocketEvent(UDPSocketEvent e) |
johnson6987 | 0:8101522bb770 | 132 | { |
johnson6987 | 0:8101522bb770 | 133 | switch (e) { |
johnson6987 | 0:8101522bb770 | 134 | case UDPSOCKET_READABLE: //The only event for now |
johnson6987 | 0:8101522bb770 | 135 | char buf[256] = {0}; |
johnson6987 | 0:8101522bb770 | 136 | Host host; |
johnson6987 | 0:8101522bb770 | 137 | while ( int len = UDP.recvfrom( buf, 255, &host ) ) { |
johnson6987 | 0:8101522bb770 | 138 | if ( len <= 0 ) |
johnson6987 | 0:8101522bb770 | 139 | break; |
johnson6987 | 0:8101522bb770 | 140 | UDP_queue.Put(buf); |
johnson6987 | 0:8101522bb770 | 141 | } |
johnson6987 | 0:8101522bb770 | 142 | break; |
johnson6987 | 0:8101522bb770 | 143 | } |
johnson6987 | 0:8101522bb770 | 144 | } |
johnson6987 | 0:8101522bb770 | 145 | |
johnson6987 | 0:8101522bb770 | 146 | |
johnson6987 | 0:8101522bb770 | 147 | int main() |
johnson6987 | 0:8101522bb770 | 148 | { |
johnson6987 | 0:8101522bb770 | 149 | |
johnson6987 | 0:8101522bb770 | 150 | eth = new EthernetNetIf( |
johnson6987 | 0:8101522bb770 | 151 | IpAddr(IpAddr(192,168,1,102)), // My IP Address |
johnson6987 | 0:8101522bb770 | 152 | IpAddr(IpAddr(255,255,255,0)), //Network Mask |
johnson6987 | 0:8101522bb770 | 153 | IpAddr(IpAddr(192,168,1,2)), //Gateway |
johnson6987 | 0:8101522bb770 | 154 | IpAddr(IpAddr()) //DNS |
johnson6987 | 0:8101522bb770 | 155 | ); |
johnson6987 | 0:8101522bb770 | 156 | Robot.setIp(IpAddr(192,168,1,102)); |
johnson6987 | 0:8101522bb770 | 157 | Robot.setPort(12345); |
johnson6987 | 0:8101522bb770 | 158 | |
johnson6987 | 0:8101522bb770 | 159 | eth->setup(); |
johnson6987 | 0:8101522bb770 | 160 | UDP.setOnEvent(&onUDPSocketEvent); |
johnson6987 | 0:8101522bb770 | 161 | UDP.bind(Robot); |
johnson6987 | 0:8101522bb770 | 162 | NetPoll.attach_us(&Net::poll,500); |
johnson6987 | 0:8101522bb770 | 163 | |
johnson6987 | 0:8101522bb770 | 164 | ErrorTime.start(); |
johnson6987 | 0:8101522bb770 | 165 | |
johnson6987 | 2:6383a282c70c | 166 | right(); |
johnson6987 | 2:6383a282c70c | 167 | forward(1); |
johnson6987 | 2:6383a282c70c | 168 | right(); |
johnson6987 | 2:6383a282c70c | 169 | forward(1); |
johnson6987 | 2:6383a282c70c | 170 | right(); |
johnson6987 | 2:6383a282c70c | 171 | forward(1); |
johnson6987 | 2:6383a282c70c | 172 | right(); |
johnson6987 | 2:6383a282c70c | 173 | forward(1); |
johnson6987 | 2:6383a282c70c | 174 | left(); |
johnson6987 | 2:6383a282c70c | 175 | left(); |
johnson6987 | 2:6383a282c70c | 176 | backwards(.2); |
johnson6987 | 2:6383a282c70c | 177 | |
johnson6987 | 0:8101522bb770 | 178 | while (1) { |
johnson6987 | 0:8101522bb770 | 179 | if ( UDP_queue.Get(messageBufferIncoming))messageProcess(); |
johnson6987 | 0:8101522bb770 | 180 | if (ErrorTime.read()>.2) { |
johnson6987 | 2:6383a282c70c | 181 | Mr=.50; |
johnson6987 | 2:6383a282c70c | 182 | Ml=0.5; |
johnson6987 | 0:8101522bb770 | 183 | for (int x=0; x<8; x++)leds[x]=0; |
johnson6987 | 0:8101522bb770 | 184 | ErrorTime.reset(); |
johnson6987 | 0:8101522bb770 | 185 | } |
johnson6987 | 0:8101522bb770 | 186 | |
johnson6987 | 0:8101522bb770 | 187 | } |
johnson6987 | 0:8101522bb770 | 188 | } |
johnson6987 | 0:8101522bb770 | 189 |