WirelessComs For David
Dependencies: EthernetNetIf Queue Servo mbed
Diff: main.cpp
- Revision:
- 3:9844ec849c12
- Parent:
- 0:8101522bb770
--- a/main.cpp Thu Feb 21 22:51:08 2013 +0000 +++ b/main.cpp Mon Jul 29 20:06:57 2013 +0000 @@ -4,17 +4,23 @@ #include "queue.h" #include "Servo.h" +BusOut myleds(LED1, LED2, LED3, LED4); Ticker NetPoll; Timer ErrorTime; -Queue UDP_queue(256,20); +Timer systemtime; +Queue UDP_queue(256,1); Serial pc(USBTX, USBRX); EthernetNetIf * eth; UDPSocket UDP; Host Robot; -Servo ML(p21); -Servo MR(p22); +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)}; +//DigitalOut leds[] = {(p10),(p11),(p12),(p13),(p14),(p15),(p16),(p17)}; char messageBufferIncoming[256]; @@ -23,11 +29,40 @@ float button[12]; }; +void dtime(int x){ +myleds=x; +//pc.printf("%d - time=%f\n",x,systemtime.read()); +} + +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) { - +dtime(1); ErrorTime.reset(); // pc.printf("%s\r\n",messageBufferIncoming); sscanf(messageBufferIncoming,"%f%f%f%f",\ @@ -35,8 +70,8 @@ &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]); +dtime(2); + // 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],\ @@ -58,43 +93,65 @@ */ - /* - float A=Joy.axis[1]; - float B=Joy.axis[0]; - if (A<.1 && B<.1 && B>-.1 && A>-.1) { - MR=.5; - ML=.5; + + float Y=Joy.axis[1]; + float X=Joy.axis[0]; + Arm=Joy.axis[2]; + Elbow=(Joy.axis[3]+1)/2.0; + Y=-Y; + dtime(3); + + if (X<0.1 && Y<0.1 && X>-0.1 && Y>-0.1) { + Ml=0.5; + Mr=0.5; + dtime(4); } else { - - if (A>.1 || A<-.1) { - ML=1-(A+1)/2; - MR=(A+1)/2; + dtime(5); + if (Y>0 && X>0) { //quadrant 1 + ML=sqrt((X*X)+(Y*Y)); + MR=X-Y; + } + if (Y>0 && X<0) { //quadrant 2 + ML=Y+X; + MR=-sqrt((X*X)+(Y*Y)); + } + if (Y<0 && X<0) { //quadrant 3 + ML=-sqrt((X*X)+(Y*Y)); + MR=X-Y; } - if (B>.25 || B<-.25) { - ML=1-(B+1)/2; - MR=1-(B+1)/2; + if (Y<0 && X>0) { //quadrant 4 + ML=X+Y; + MR=sqrt((X*X)+(Y*Y)); } + Ml=((ML*ML*ML)+1)/2.0; + Mr=((MR*MR*MR)+1)/2.0; } + dtime(6); + //pc.printf("X=%f Y=%f ML=%f MR=%f\r\n", X,Y,Ml.read(),Mr.read()); + /* - */ - ML=1; - MR=0; + /*ML=1; + //MR=0; for (int x=0; x<8; x++) { leds[x]=(bool)Joy.button[x]; - } + dtime(7); + }*/ } void onUDPSocketEvent(UDPSocketEvent e) { +dtime(14); switch (e) { case UDPSOCKET_READABLE: //The only event for now char buf[256] = {0}; Host host; - while ( int len = UDP.recvfrom( buf, 255, &host ) ) { + while ( int len = UDP.recvfrom( buf, 255, &host ) ) { if ( len <= 0 ) break; UDP_queue.Put(buf); } + // UDP.recvfrom( messageBufferIncoming, 255, &host ); + //messageProcess(); break; } } @@ -102,7 +159,7 @@ int main() { - + systemtime.start(); eth = new EthernetNetIf( IpAddr(IpAddr(192,168,1,102)), // My IP Address IpAddr(IpAddr(255,255,255,0)), //Network Mask @@ -122,12 +179,12 @@ 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; + dtime(15); + Mr=.50; + Ml=0.5; + ErrorTime.reset(); } } } -