kyle johnson / Mbed 2 deprecated WirelessComs

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?

UserRevisionLine numberNew 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