kyle johnson / Mbed 2 deprecated WirelessComs

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?

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