kyle johnson / Mbed 2 deprecated WirelessComs

WirelessComs For David

Dependencies:   EthernetNetIf Queue Servo mbed

main.cpp@3:9844ec849c12, 2013-07-29 (annotated)

Committer:
johnson6987
Date:
Mon Jul 29 20:06:57 2013 +0000
Revision:
3:9844ec849c12
Parent:
0:8101522bb770 
Final Code after Adam H

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