Diff: main.cpp

Revision:

2:2eb3f4362288

Parent:

1:da09e6a94937

Child:

3:a0cd142687d0

--- a/main.cpp	Mon Sep 06 15:22:33 2010 +0000
+++ b/main.cpp	Thu May 06 13:41:44 2021 +0000
@@ -1,4 +1,3 @@
-
 // TCP Echo server
 // 2010/9/7
 
@@ -30,8 +29,12 @@
 #include "mbed.h"
 #include "EthernetNetIf.h"
 #include "TCPSocket.h"
+#include "Json.h"
 
-DigitalOut led4(LED4, "led4");
+PwmOut led1(LED1, "led1");
+PwmOut led4(LED4, "led4");
+PwmOut left_thrust_pin(p21);
+PwmOut right_thrust_pin(p22);
 
 // EthernetNetIf eth;
 EthernetNetIf eth(
@@ -47,6 +50,19 @@
 TCPSocket* pConnectedSock; // for ConnectedSock
 Host client;
 TCPSocketErr err;
+float left_motor_thrust = 0.0;
+float right_motor_thrust = 0.0;
+
+float clamp(const float & value, const float & min = 0.0, const float & max = 1.0)
+{
+    if(value <= min) {
+        return min;
+    }
+    if(value >= max) {
+        return max;
+    }
+    return value;
+}
 
 void onConnectedTCPSocketEvent(TCPSocketEvent e)
 {
@@ -64,12 +80,42 @@
         printf("TCP Socket Readable\r\n");
        // Read in any available data into the buffer
        char buff[128];
+       bool succees = false;
        while ( int len = pConnectedSock->recv(buff, 128) ) {
-       // And send straight back out again
-           pConnectedSock->send(buff, len);
+           Json json(buff, len);
+           if (!json.isValidJson()) {
+                succees = false;
+           }
+           else {
+               int left_index = json.findKeyIndexIn ("left", 0 );
+               int right_index = json.findKeyIndexIn ("right", 0 );
+               int left_value_index = json.findChildIndexOf(left_index, -1);
+               int right_value_index = json.findChildIndexOf(right_index, -1);
+               if(left_index == -1 || right_index == -1) {
+                   succees = false;
+               }
+               else {
+                   if(left_value_index > 0 && right_value_index > 0) {
+                       if(json.tokenNumberValue(left_value_index, left_motor_thrust) == 0 && 
+                          json.tokenNumberValue(right_value_index, right_motor_thrust) == 0) {
+                              succees = true;
+                       }
+                   }
+               }
+           }
+           /*
+           left_motor_thrust = (unsigned char)buff[0];
+           right_motor_thrust = (unsigned char)buff[1];
+           */
+           // And send straight back out again
+           // pConnectedSock->send(buff, len);
            buff[len]=0; // make terminater
            printf("Received&Wrote:%s\r\n",buff);
        }
+       if(!succees) {
+           left_motor_thrust = 0.0;
+           right_motor_thrust = 0.0;   
+       }
        break;
     case TCPSOCKET_CONTIMEOUT:
         printf("TCP Socket Timeout\r\n");
@@ -146,8 +192,11 @@
      };
 }
 
-
 int main() {
+  led1.period(0.002);
+  led4.period(0.002);
+  left_thrust_pin.period(0.002);
+  right_thrust_pin.period(0.002);
   printf("\r\n");
   printf("Setting up...\r\n");
   EthernetErr ethErr = eth.setup();
@@ -182,14 +231,21 @@
  
   Timer tmr;
   tmr.start();
+  float timer_count = 0.0;
 
   while(true)
   {
     Net::poll();
-    if(tmr.read() > 0.2) // sec
+    if(tmr.read() > 0.001) // sec
     {
-      led4=!led4; //Show that we are alive
-      tmr.reset();
+      timer_count = timer_count + 0.001;
+      if(timer_count >= 1.0) {
+          timer_count = 0.0;
+          led1 = 0.5 + clamp(left_motor_thrust, -1, 1) * 0.5;
+          led4 = 0.5 + clamp(right_motor_thrust, -1, 1) * 0.5;
+          left_thrust_pin = 0.75 + clamp(left_motor_thrust, -1, 1) * 0.2;
+          right_thrust_pin = 0.75 + clamp(right_motor_thrust, -1, 1) * 0.2;
+      }
     }
   }  
 }