add ability to build and upload sketches from the bash command line (via beta ino2cpp...
[zumo-cc3200/zumo-cc3200.git] / src / Energia / ZumoTest / apLoop.ino
1 /*
2  *  ======== apLoop ========
3  *  This sketch starts a network and listens on port PORTNUM for
4  *  command that can control the zumo motors.
5  *
6  *  The name and password of the network and the port number of the server
7  *  (always at IP address 192.168.1.1) can be changed below.
8  */
10 #include <WiFi.h>
11 #include <ti/sysbios/knl/Task.h>
13 #include <string.h>
15 /* name of the network and its password */
16 static const char ssid[] = "zumo-test";
17 static const char wifipw[] = "password";
19 /* port number of the server listening for commands at 192.168.1.1 */
20 #define PORTNUM 8080
22 /* max and min PWM values to be sent to the motorLoop */
23 #define PWM_MAX 400
24 #define PWM_MIN 100
26 /* create data server on port PORTNUM */
27 static WiFiServer server(PORTNUM);
29 static void doCMD(char *buffer, WiFiClient client);
31 /*
32  *  ======== apSetup ========
33  */
34 void apSetup()
35 {
36     Serial.begin(9600);
38     /* set priority of this task to be lower than other tasks */
39     Task_setPri(Task_self(), 1);
41     /* startup a new network and get the first IP address: 192.168.1.1 */
42     Serial.print("Starting a new network: "); Serial.println(ssid);
43     WiFi.beginNetwork((char *)ssid, (char *)wifipw);
44     while (WiFi.localIP() == INADDR_NONE) {
45         Serial.print(".");
46         delay(300);
47     }
49     /* startup the command server on port PORTNUM */
50     server.begin();
51     Serial.print("dataserver started on port "); Serial.println(PORTNUM);
52 }
54 /*
55  *  ======== apLoop ========
56  */
57 void apLoop()
58 {
59     /* Did a client connect/disconnect since the last time we checked? */
60     if (WiFi.getTotalDevices() > 0) {
62         /* listen for incoming clients */
63         WiFiClient client = server.available();
64         if (client) {
66             /* if there's a client, read and process commands */
67             static char buffer[64] = {0};
68             int bufLen = 0;
70             /* while connected to the client, read commands and send results */
71             while (client.connected()) {
72                 /* if there's a byte to read from the client .. */
73                 if (client.available()) {
74                     /* copy it to the command buffer, byte at a time */
75                     char c = client.read();
77                     /* ignore bogus characters */
78                     if (c == '\0' || c == '\r') continue;
79                     
80                     /* never overrun the command buffer */
81                     if (bufLen >= (int)(sizeof (buffer))) { 
82                         bufLen = sizeof (buffer) - 1;
83                     }
84                     buffer[bufLen++] = c;
86                     /* if there's a new line, we have a complete command */
87                     if (c == '\n') {
88                         doCMD(buffer, client);
89                         
90                         /* reset command buffer index to get next command */
91                         bufLen = 0;
92                     }
93                 }
94             }
96             /* client disconnected or timed out, close the connection */
97             client.flush();
98             client.stop();
100             /* disconnect => implicitly stop the motor */
101             motorCmd = ' ';
102         }
103     }
105     /* check for new connections 2 times per second */
106     delay(500);
109 /*
110  *  ======== getVelocityToPWM ========
111  *  Convert raw motor velocity values to PWM values
112  *  
113  *  Raw motor velocity values range from +/-100; 0 being off
114  *
115  *  A conversion can be done by
116  *  pwmOutput = factor * (logical velocity) + pwm_min
117  *  factor = (pwm_max / pwm_min)
118  */
119 static int getVelocityToPWM(int velocity)
121    velocity *= (PWM_MAX / PWM_MIN);
122    
123    if (velocity > 0) {
124        velocity = (velocity < PWM_MIN) ? PWM_MIN : velocity;
125    }
126    else if (velocity < 0) {
127        velocity = (velocity > -PWM_MIN) ? -PWM_MIN : velocity;
128    }
130    return (velocity);
133 /*
134  *  ======== doCMD ========
135  */
136 static void doCMD(char *buffer, WiFiClient client)
138     static char report[80];
139     int left = (~0);
140     int right = (~0);
141     char newCMD = ' ';
142     UInt savePri;
143     
144     /* parse the command in buffer[] */
145     switch (buffer[0]) {
146         /* handle raw motor control commands: "l<left>, r<right>" */
147         case 'l':
148             left = strtol(buffer + 1, &buffer, 10);
149             while (buffer[0] != '\n') {
150                 if (buffer[0] == 'r') {
151                     right = strtol(buffer + 1, &buffer, 10);
152                     break;
153                 }
154                 buffer++;
155             }
157             newCMD = 'r';
158             break;
159             
160         case 'w':
161         case 'a':
162         case 's':
163         case 'd':
164             newCMD = buffer[0];
165             break;
166                 
167         default:
168             break;
169     }
170     
171     /* atomically update motor control variables */
172     savePri = Task_setPri(Task_self(), Task_numPriorities - 1);
173     if (right != (~0)) {
174         motorRight = getVelocityToPWM(right);
175     }
176     if (left != (~0)) {
177         motorLeft = getVelocityToPWM(left);
178     }
179     motorCmd = newCMD;
180     Task_setPri(Task_self(), savePri);
181     
182     /* send current IMU data */
183     System_snprintf(report, sizeof(report),
184         "A: %6d %6d %6d G: %6d %6d %6d M: %6d %6d %6d",
185                     imuCompass.a.x, imuCompass.a.y, imuCompass.a.z,
186                     imuGyro.g.x,    imuGyro.g.y,    imuGyro.g.z, 
187                     imuCompass.m.x, imuCompass.m.y, imuCompass.m.z);
188     if (client.write((unsigned char *)report, 72) != 72) {
189         Serial.println("Error: reply failed, status != 72");
190     }