WirelessComs For David
Dependencies: EthernetNetIf Queue Servo mbed
main.cpp
- Committer:
- johnson6987
- Date:
- 2013-05-13
- Revision:
- 2:6383a282c70c
- Parent:
- 0:8101522bb770
- Child:
- 4:bae1c47dc112
File content as of revision 2:6383a282c70c:
#include "mbed.h" #include "EthernetNetIf.h" #include "UDPSocket.h" #include "queue.h" #include "Servo.h" Ticker NetPoll; Timer ErrorTime; Queue UDP_queue(256,20); Serial pc(USBTX, USBRX); EthernetNetIf * eth; UDPSocket UDP; Host Robot; Servo Ml(p21); Servo Mr(p22); Servo Arm(p23); Servo Elbow(p24); float MR=.5; float ML=.5; DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)}; char messageBufferIncoming[256]; struct Joystick { float axis[4]; float button[12]; }; void right(){ Ml=1; Mr=1; wait(.25); } void left(){ Ml=0; Mr=0; wait(.25); } void forward(float time){ Ml=1; Mr=0; wait(time); } void backwards(float time){ Ml=0; Mr=1; wait(time); } Joystick Joy; void messageProcess(void) { ErrorTime.reset(); // pc.printf("%s\r\n",messageBufferIncoming); sscanf(messageBufferIncoming,"%f%f%f%f",\ &Joy.axis[0],\ &Joy.axis[1],\ &Joy.axis[2],\ &Joy.axis[3]); // pc.printf("%f %f %f %f\r\n", Joy.axis[0],Joy.axis[1],Joy.axis[2],Joy.axis[3]); /* sscanf(messageBufferIncoming,"%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f%f",\ &Joy.axis[0],\ &Joy.axis[1],\ &Joy.axis[2],\ &Joy.axis[3],\ &Joy.button[0],\ &Joy.button[1],\ &Joy.button[2],\ &Joy.button[3],\ &Joy.button[4],\ &Joy.button[5],\ &Joy.button[6],\ &Joy.button[7],\ &Joy.button[8],\ &Joy.button[9],\ &Joy.button[10],\ &Joy.button[11]); */ float Y=Joy.axis[1]; float X=Joy.axis[0]; Arm=Joy.axis[2]; Elbow=(Joy.axis[3]+1)/2.0; Y=-Y; if (X<.1 && Y<.1 && X>-.1 && Y>-.1) { Ml=.5; Mr=.5; } else { if (Y>0 && X>0) { ML=sqrt((X*X)+(Y*Y)); MR=Y-X; } if (Y>0 && X<0) { ML=Y+X; MR=sqrt((X*X)+(Y*Y)); } if (Y<0 && X<0) { ML=Y-X; MR=-sqrt((X*X)+(Y*Y)); } if (Y<0 && X>0) { ML=-sqrt((X*X)+(Y*Y)); MR=Y+X; } Ml=(ML+1)/2.0; Mr=(1-MR)/2.0; } pc.printf("X=%f Y=%f ML=%f MR=%f\r\n", X,Y,Ml.read(),Mr.read()); /* /*ML=1; MR=0; for (int x=0; x<8; x++) { leds[x]=(bool)Joy.button[x]; }*/ } void onUDPSocketEvent(UDPSocketEvent e) { switch (e) { case UDPSOCKET_READABLE: //The only event for now char buf[256] = {0}; Host host; while ( int len = UDP.recvfrom( buf, 255, &host ) ) { if ( len <= 0 ) break; UDP_queue.Put(buf); } break; } } int main() { eth = new EthernetNetIf( IpAddr(IpAddr(192,168,1,102)), // My IP Address IpAddr(IpAddr(255,255,255,0)), //Network Mask IpAddr(IpAddr(192,168,1,2)), //Gateway IpAddr(IpAddr()) //DNS ); Robot.setIp(IpAddr(192,168,1,102)); Robot.setPort(12345); eth->setup(); UDP.setOnEvent(&onUDPSocketEvent); UDP.bind(Robot); NetPoll.attach_us(&Net::poll,500); ErrorTime.start(); right(); forward(1); right(); forward(1); right(); forward(1); right(); forward(1); left(); left(); backwards(.2); while (1) { if ( UDP_queue.Get(messageBufferIncoming))messageProcess(); if (ErrorTime.read()>.2) { Mr=.50; Ml=0.5; for (int x=0; x<8; x++)leds[x]=0; ErrorTime.reset(); } } }