Move robofish firmware into separate repository

.gitignore

+.vscode
\ No newline at end of file

README.md

+# Flashing the firmware
+
+- Install the arduino IDE
+- Install the [drivers for the nodemcu](https://github.com/nodemcu/nodemcu-devkit/blob/master/Drivers/CH341SER_WINDOWS.zip)
+- Arduino IDE -> File -> Settings   
+  Set Additional Boards Manager URLS:  
+  http://arduino.esp8266.com/stable/package_esp8266com_index.json
+- Arduino IDE -> Tools -> Boards Manager
+  Search for "esp", install "esp8266" (tested version is 2.5.0-beta3)
+- Make sure the settings are as depicted:  
+  ![Menu](Menu.png)
+- Put the robot's ID in robofish.h and remember not to commit your changes to git:  
+  #define ROBOT_ID 112
+- Adjust the code as needed (e.g. wifi password)
+- Upload the code
\ No newline at end of file

firmware/.vscode/arduino.json

+{
+    "board": "esp8266:esp8266:nodemcuv2",
+    "configuration": "xtal=80,vt=flash,exception=disabled,ssl=all,eesz=4M,ip=lm2f,dbg=Disabled,lvl=None____,wipe=none,baud=57600",
+    "sketch": "common.ino",
+    "programmer": "AVRISP mkII",
+    "port": "/dev/ttyUSB0",
+    "arduino.logLevel": "verbose"
+}
\ No newline at end of file

firmware/.vscode/c_cpp_properties.json

+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "/home/mm/.arduino15/packages/esp8266/tools/**",
+                "/home/mm/.arduino15/packages/esp8266/hardware/esp8266/2.5.2/**"
+            ],
+            "forcedInclude": [],
+            "intelliSenseMode": "gcc-x64",
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c11",
+            "cppStandard": "c++17"
+        }
+    ],
+    "version": 4
+}
\ No newline at end of file

firmware/.vscode/settings.json

+{
+	"editor.formatOnSave": false,
+	"files.associations": {
+		"array": "cpp",
+		"strstream": "cpp",
+		"bit": "cpp",
+		"*.tcc": "cpp",
+		"cctype": "cpp",
+		"clocale": "cpp",
+		"cmath": "cpp",
+		"cstdarg": "cpp",
+		"cstddef": "cpp",
+		"cstdint": "cpp",
+		"cstdio": "cpp",
+		"cstdlib": "cpp",
+		"cstring": "cpp",
+		"cwchar": "cpp",
+		"cwctype": "cpp",
+		"deque": "cpp",
+		"unordered_map": "cpp",
+		"vector": "cpp",
+		"exception": "cpp",
+		"fstream": "cpp",
+		"functional": "cpp",
+		"initializer_list": "cpp",
+		"iosfwd": "cpp",
+		"iostream": "cpp",
+		"istream": "cpp",
+		"limits": "cpp",
+		"memory": "cpp",
+		"new": "cpp",
+		"optional": "cpp",
+		"ostream": "cpp",
+		"sstream": "cpp",
+		"stdexcept": "cpp",
+		"streambuf": "cpp",
+		"string": "cpp",
+		"string_view": "cpp",
+		"system_error": "cpp",
+		"cinttypes": "cpp",
+		"type_traits": "cpp",
+		"tuple": "cpp",
+		"typeinfo": "cpp",
+		"utility": "cpp"
+	}
+}
\ No newline at end of file

firmware/Packet.h

+#pragma once
+
+// MSVC up to version 2008 do not ship with stdint.h
+// (which defines uintX_t etc.), so we need to include
+// one, e.g. from https://code.google.com/p/msinttypes/
+#if defined _MSC_VER && _MSC_VER < 1600
+#include "stdint.h"
+#else
+#if defined _MSC_VER
+//#define uint8_t unsigned __int8
+#endif
+#include <stdint.h>
+#endif
+
+#include <stdint.h>
+
+#include <stdlib.h>
+
+const uint8_t protocolVersion = 2;
+
+// Packet types
+const char DiscoveryRequest = 'D';
+const char MotorRequest     = 'M';
+const char TimingRequest    = 'T';
+const char VoltageRequest   = 'V';
+const char LedOnRequest     = 'L';
+const char BoostCfgRequest  = 'B';
+const char DiscoveryReply   = 'd'; // deprecated, only used by old protocol
+const char DiscoveryVersionReply = 'e'; // new protocol reply
+const char TimingReply           = 't';
+const char VoltageReply          = 'v';
+
+// Bit set -> reverse motor
+const uint8_t LeftReverse  = 0x01;
+const uint8_t RightReverse = 0x02;
+
+// Set memory alignment to one byte so that this struct
+// is really the same on all platforms
+#ifdef _MSC_VER
+#pragma pack(push, 1)
+#endif
+
+struct Packet
+{
+	char type;
+	union {
+		struct
+		{
+			uint8_t directions;
+			uint8_t left[2];
+			uint8_t right[2];
+		} motor;
+
+		char    voltStr[3];
+		uint8_t robotID;
+		struct
+		{
+			uint8_t robotID;
+			uint8_t version;
+		} discovery;
+
+	} data;
+}
+#if defined __GNUC__ || defined __MINGW32__
+__attribute__((packed, aligned(1)))
+#endif
+;
+
+void inline writeDiscoveryRqPacket(void* dest)
+{
+	Packet* p = (Packet*) dest;
+	p->type   = DiscoveryRequest;
+}
+
+void inline writeMotorRqPacket(void*   dest,
+                               int32_t leftSpeed,
+                               int32_t rightSpeed)
+{
+	Packet* p                = (Packet*) dest;
+	p->type                  = MotorRequest;
+	p->data.motor.directions = (leftSpeed > 0 ? LeftReverse : 0x00) |
+	                           (rightSpeed > 0 ? RightReverse : 0x00);
+	rightSpeed             = abs(rightSpeed);
+	leftSpeed              = abs(leftSpeed);
+	p->data.motor.left[0]  = (uint8_t)(leftSpeed % 256);
+	p->data.motor.left[1]  = (uint8_t)(leftSpeed / 256);
+	p->data.motor.right[0] = (uint8_t)(rightSpeed % 256);
+	p->data.motor.right[1] = (uint8_t)(rightSpeed / 256);
+}
+
+void inline writeTimingRqPacket(void* dest)
+{
+	Packet* p = (Packet*) dest;
+	p->type   = TimingRequest;
+}
+
+void inline writeVoltageRqPacket(void* dest)
+{
+	Packet* p = (Packet*) dest;
+	p->type   = VoltageRequest;
+}
+
+void inline writeLedOnRq(void* dest)
+{
+	Packet* p = (Packet*) dest;
+	p->type   = LedOnRequest;
+}
+
+void inline writeDiscoveryRePacket(void* dest, uint8_t robotID)
+{
+	Packet* p                 = (Packet*) dest;
+	p->type                   = DiscoveryVersionReply;
+	p->data.discovery.robotID = robotID;
+	p->data.discovery.version = protocolVersion;
+}
+
+void inline writeTimingRePacket(void* dest)
+{
+	Packet* p = (Packet*) dest;
+	p->type   = TimingReply;
+}
+
+void inline writeVoltageRePacket(void* dest, double voltage)
+{
+	Packet* p = (Packet*) dest;
+	p->type   = VoltageReply;
+	char voltBuf[4];
+	;
+	snprintf(voltBuf, 4, "%f", voltage);
+	p->data.voltStr[0] = voltBuf[0];
+	p->data.voltStr[1] = voltBuf[2];
+	p->data.voltStr[2] = voltBuf[3];
+}
+
+#ifdef _MSC_VER
+#pragma pack(pop)
+#endif

firmware/consts.h

+#include <Arduino.h>
+#include "robofish.h"
+
+// const int M1 = 5;         // Arduino PWM Speed Control
+// const int D1 = 2;         // Arduino Digital Direction Control
+// const int M2 = 6;         // Arduino PWM Speed Control
+// const int D2 = 8;         // Arduino Digital Direction Control
+#define NODEMCU 1
+
+#ifdef NODEMCU
+// NodeMCU Mapping
+static const uint8_t MCU_D0__  = 16;
+static const uint8_t MCU_D1__  = 5;
+static const uint8_t MCU_D2__  = 4;
+static const uint8_t MCU_D3__  = 0;
+static const uint8_t MCU_D4__  = 2;
+static const uint8_t MCU_D5__  = 14;
+static const uint8_t MCU_D6__  = 12;
+static const uint8_t MCU_D7__  = 13;
+static const uint8_t MCU_D8__  = 15;
+static const uint8_t MCU_D9__  = 3;
+static const uint8_t MCU_D10__ = 1;
+#endif
+
+// GPIO4 == 19
+// GPIO5 == 20
+#ifdef NODEMCU
+// D3=Dir1
+// D4=Forw1
+// D7=Dir2
+// D8=Forw2
+const int M1_ = MCU_D3__;  // Arduino PWM Speed Control
+const int D1_ = MCU_D4__;  // Arduino Digital Direction Control
+const int M2_ = MCU_D8__;  // Arduino PWM Speed Control
+const int D2_ = MCU_D7__;  // Arduino Digital Direction Control
+#define ANALOG_PORT_BAT A0 // A0
+#else
+const int M1_ = 12; // Arduino PWM Speed Control
+const int D1_ = 13; // Arduino Digital Direction Control
+const int M2_ = 14; // Arduino PWM Speed Control
+const int D2_ = 16; // Arduino Digital Direction Control
+#define A2 2
+#define ANALOG_PORT_BAT A2 // A2
+#endif
+
+// const int LedPin  = 2;    // LED output pin
+// const int LedPort = 9;
+
+const int    BatteryPort = ANALOG_PORT_BAT;
+const double BatteryMax  = 8.3;

firmware/debug.h

+#define DEBUG_LEVEL 2 // 0: silent, 1: some output, 2: verbose output
+
+#if DEBUG_LEVEL > 0
+#define DBG_MSG_LN(s) Serial.println(s)
+#define DBG_MSG(s) Serial.print(s)
+#else
+#define DBG_MSG_LN(s)
+#define DBG_MSG(s)
+#endif
+
+#if DEBUG_LEVEL > 1
+#define V_DBG_MSG_LN(s) Serial.println(s)
+#define V_DBG_MSG(s) Serial.print(s)
+#else
+#define V_DBG_MSG_LN(s)
+#define V_DBG_MSG(s)
+#endif

firmware/firmware.ino

+/*
+ * UDP endpoint
+ *
+ * A simple UDP endpoint example using the WiShield 1.0
+ */
+
+#include <limits>
+
+#include <SPI.h>
+#include <ESP8266WiFi.h>
+#include <WiFiUdp.h>
+
+ADC_MODE(ADC_TOUT)
+
+extern "C" { // required for read Vdd Voltage
+#include "user_interface.h"
+}
+
+#include <Arduino.h>
+#include "robofish.h"
+#include "debug.h"
+#include "wifi_config.h"
+#include "consts.h"
+#include "Packet.h"
+#include "modulo.h"
+
+// Battery
+int    batteryValue       = 0;
+double currentBattVoltage = 0.0;
+
+// Speeds
+uint16_t speedLeft  = 0;
+uint16_t speedRight = 0;
+uint8_t  directions = 0x00;
+
+WiFiUDP       Udp;
+int           status = WL_IDLE_STATUS; // the Wifi radio's status
+char          packetBuffer[255];       // buffer to hold incoming packet
+char          replyBuffer[255];        // buffer to hold incoming packet
+unsigned long lastPacketAt =
+   0; // time in milliseconds the last packet arrived at
+
+void sendVoltage()
+{
+	// We need to use this SDK function for reading the ADC.
+	// analog_read from Arduino SKD only delivers binary values 0 or 1024.
+	batteryValue = system_adc_read();
+	// Debug out:
+	// Serial.println(batteryValue);
+
+	// Previously this formula was used. By logic this should work, but does not
+	// reflect the property of the voltage divider properly.
+	// currentBattVoltage = 6.0 + (batteryValue - BATT_VAL_6V) * 3.0 /
+	// (BATT_VAL_9V - BATT_VAL_6V);
+
+	// (we assume 8V == 100% and 6V == 0%, but the GUI makes the actual mapping)
+	// Measured value from ADC is p and k is the mapping from read value to
+	// actual voltage. k was experimentally found, as the output does not scale
+	// linearly as expected from 1..1024 to 0V..1V Calculate upper PWM according
+	// to Voltage Divider formula:
+	//  Vo = (V*R2)/(R1+R2),  R2 = 1k,  R1 = 10k, k = 1064/1000
+	//  Vo = p * k
+	//  p * k = (V*R2)/(R1+R2)
+	//  p * k * (R1+R2) = V*R2
+	//  (p * k * (R1+R2)) / R2 = V
+	// Note: Accuracy seems to be +/- 0.05V. Tendency is to -0.04V
+	float k            = 1064.0 / 1000.0;
+	float r1           = 10000.0;
+	float r2           = 1000.0;
+	float p            = ((float) batteryValue) / 1000.0;
+	currentBattVoltage = ((p * (r1 + r2)) / r2) * k;
+
+	writeVoltageRePacket(replyBuffer, currentBattVoltage);
+
+	// send a reply, to the IP address and port that sent us the packet we
+	// received
+	Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
+	Udp.write(replyBuffer, sizeof(Packet));
+	Udp.endPacket();
+	V_DBG_MSG_LN("Voltage packet send");
+}
+
+void inline sendTimingReply()
+{
+	writeTimingRePacket(
+	   replyBuffer); // send a reply, to the IP address and port that sent us
+	                 // the packet we received
+	Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
+	Udp.write(replyBuffer, sizeof(Packet));
+	Udp.endPacket();
+
+	V_DBG_MSG_LN("Timing packet send");
+}
+
+void inline sendDiscoveryReply()
+{
+	writeDiscoveryRePacket(
+	   replyBuffer,
+	   ROBOT_ID); // send a reply, to the IP address and port that sent us the
+	              // packet we received
+	Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
+	Udp.write(replyBuffer, sizeof(Packet));
+	Udp.endPacket();
+
+	V_DBG_MSG_LN("Discovery packet reply send");
+}
+
+void setup()
+{
+	pinMode(D1_, OUTPUT);
+	pinMode(D2_, OUTPUT);
+	digitalWrite(D1_, LOW); // LOW = clockwise
+	digitalWrite(D2_, LOW); // HIGH = counter-clockwise
+	analogWrite(M1_, 0);
+	analogWrite(M2_, 0);
+
+	Serial.begin(115200);
+	DBG_MSG_LN("");
+	DBG_MSG_LN("(Baud switched)");
+	DBG_MSG_LN("Starting setup");
+
+	// attempt to connect to Wifi network:
+
+	status = WiFi.begin(ssid, pass);
+	DBG_MSG_LN("Init...");
+
+	// Wait for connection
+	while (WiFi.status() != WL_CONNECTED) {
+		delay(500);
+		Serial.print(".");
+	}
+	DBG_MSG_LN(WiFi.localIP());
+
+	DBG_MSG_LN("WLAN initialization complete");
+
+	// analogReference(DEFAULT); TODO ???
+	pinMode(BatteryPort, INPUT);
+
+	setMotorPWM(false, 0, false, 0);
+
+	DBG_MSG_LN("Setup complete");
+
+	Udp.begin(12344);
+}
+
+void loop()
+{
+	// if there's data available, read a packet
+	int packetSize = Udp.parsePacket();
+	if (packetSize) {
+		lastPacketAt = millis();
+
+		// read the packet into packetBufffer
+		int len = Udp.read(packetBuffer, 255);
+		if (len > 0) {
+			packetBuffer[len] = 0;
+		}
+
+		Packet* packet = (Packet*) packetBuffer;
+
+		// V_DBG_MSG_LN("Packet recieved");
+
+		// Examine packet
+		if (packet->type == TimingRequest) {
+			// V_DBG_MSG_LN("Received packet contains timing request");
+			sendTimingReply();
+		} else if (packet->type == MotorRequest) {
+			// V_DBG_MSG_LN("Received packet contains motor request");
+			speedLeft = packet->data.motor.left[0] +
+			            packet->data.motor.left[1] * 256;
+			speedRight = packet->data.motor.right[0] +
+			             packet->data.motor.right[1] * 256;
+			directions = packet->data.motor.directions;
+		} else if (packet->type == DiscoveryRequest) {
+			// V_DBG_MSG_LN("Discovery packet received");
+			sendDiscoveryReply();
+		} else if (packet->type == VoltageRequest) {
+			// V_DBG_MSG_LN("Received packet contains status request");
+			sendVoltage();
+		} else {
+			V_DBG_MSG_LN("Received gibberish packet");
+		}
+	} else if (speedLeft != 0 || speedRight != 0) {
+		auto const now = static_cast<long long>(millis());
+		auto const dt  = now - static_cast<long long>(lastPacketAt);
+		auto const n   = 1 + static_cast<long long>(
+                            std::numeric_limits<unsigned long>::max());
+		if (1000 < modulo(dt, n)) {
+			speedLeft  = 0;
+			speedRight = 0;
+			directions = 0;
+		}
+	}
+
+	setMotorPWM(directions & LeftReverse,
+	            speedLeft,
+	            directions & RightReverse,
+	            speedRight);
+}
+
+void setMotorPWM(bool     leftReverse,
+                 uint16_t speedLeft,
+                 bool     rightReverse,
+                 uint16_t speedRight)
+{
+	digitalWrite(D1_, leftReverse ? HIGH : LOW);
+	digitalWrite(D2_, rightReverse ? LOW : HIGH);
+
+	uint16_t pwmLeft  = (uint16_t) speedLeft;
+	uint16_t pwmRight = (uint16_t) speedRight;
+
+	// The ESP12F is doing software PWM.
+	// This can be flaky at the boundaries, so better round it then.
+	// if (pwmLeft >= 1024)
+	//    pwmLeft = 1023;
+	// if (pwmLeft < 0)
+	//    pwmLeft = 0;
+	// if (pwmRight > 1024)
+	//    pwmRight = 1023;
+	// if (pwmRight < 0)
+	//    pwmRight = 0;
+
+	// Right now everything is geared towards arduino 8-bit PWM.
+	// So we simply scale this up
+	analogWrite(M1_, pwmLeft);
+	analogWrite(M2_, pwmRight);
+}

firmware/modulo.h

+#pragma once
+
+inline long long floored_remainder(long long a, long long n)
+{
+	auto const r = a % n;
+	if (r && (a < 0) != (n < 0))
+		return r + n;
+	return r;
+}
+
+inline long long modulo(long long a, long long n)
+{
+	if (a > 0) {
+		return a % n;
+	} else if (n > 0) {
+		return floored_remainder(a, n);
+	} else {
+		return -floored_remainder(-a, n);
+	}
+}

firmware/robofish.h

+/***** ROBOT IDENTIFICATION *****/
+/* change this before uploading */
+
+#define ROBOT_ID 113
+
+// Assuming 8V == 100% and 6V == 0%
+// Calculate upper PWM according to Voltage Divider formula:
+// Vo = (V*R2)/(R1+R2),  R2 = 1k,  R1 = 10k
+// Vo_min = 0,545
+// Vo_max = 0,818
+
+#if ROBOT_ID == 101
+#define BATT_VAL_6V 545
+#define BATT_VAL_9V 727
+#define MOTOR_VOLT 5.00
+
+#elif ROBOT_ID == 102
+#define BATT_VAL_6V 532
+#define BATT_VAL_9V 710
+#define MOTOR_VOLT 5.00
+
+#elif ROBOT_ID == 107 // HU-Bot fixed up with prototype HW
+#define BATT_VAL_6V 545
+#define BATT_VAL_9V 727
+#define MOTOR_VOLT 5.00
+#elif ROBOT_ID == 108 // new NodeMCU Bot
+#define BATT_VAL_6V 545
+#define BATT_VAL_9V 727
+#define MOTOR_VOLT 5.00
+#elif ROBOT_ID >= 110 // NodeMCU + PCB
+#define BATT_VAL_6V 545
+#define BATT_VAL_9V 727
+#define MOTOR_VOLT 5.00
+
+#else
+#error "No parameters #define'd for this ROBOT_ID"
+#endif

firmware/wifi_config.h

+//#include <WiFi.h>
+#include <ESP8266WiFi.h>
+
+// the IP address for the shield:
+// IPAddress ip(192, 168, 178, ROBOT_ID);
+// IPAddress gateway(192, 168, 178, 1);
+// IPAddress subnet(255, 255, 255, 0);
+// IPAddress dns(192, 168, 178, 1);
+
+// Wireless Config
+// char ssid[] = "FRITZ!Box 7490";    // your network SSID (name)
+// char pass[] = "88774779065982215160"; // your network password (use for WPA,
+// or use as key for WEP)
+
+// the IP address for the shield:
+IPAddress ip(192, 168, 0, ROBOT_ID);
+IPAddress gateway(192, 168, 0, 1);
+IPAddress subnet(255, 255, 255, 0);
+IPAddress dns(192, 168, 0, 1);
+
+// Wireless Config
+char ssid[] = "RoboFish"; // your network SSID (name)
+char pass[] =
+   "vhr0b0134js"; // your network password (use for WPA, or use as key for WEP)

tools/robofish.py

+#!/usr/bin/env python2
+
+from ctypes import *
+import time
+import socket
+
+DISCOVERY_REQUEST = 'D'
+MOTOR_REQUEST = 'M'
+TIMING_REQUEST = 'T'
+VOLTAGE_REQUEST = 'V'
+LEDON_REQUEST = 'L'
+BOOSTCFG_REQUEST = 'B'
+
+DISCOVERY_REPLY = 'd'
+TIMING_REPLY = 't'
+VOLTAGE_REPLY = 'v'
+
+BOOST_OFF = 'O'
+BOOST_PUSH = 'P'
+BOOST_INTERLACE = 'I'
+
+LEFT_REVERSE = 0x01
+RIGHT_REVERSE = 0x02
+
+class MOTOR(Structure):
+    _pack_ = 1
+    _fields_ = [("directions", c_int8),
+                ("left", c_int8),
+                ("right", c_int8)]
+
+class BOOST(Structure):
+    _pack_ = 1
+    _fields_ = [("method", c_char),
+                ("threshold", c_int8),
+                ("boostVal", c_int8),
+                ("cycles", c_int8)]
+
+class DATA(Union):
+    _pack_ = 1
+    _fields_ = [("motor",  MOTOR),
+                ("voltStr", c_char * 3),
+                ("robotID", c_int8),
+                ("boost", BOOST)]
+
+class PACKET(Structure):
+    _pack_ = 1
+    _fields_ = [("type", c_char),
+                ("data", DATA)]
+
+def discovery_request():
+    return PACKET(DISCOVERY_REQUEST)
+
+def motor_request(leftSpeed, rightSpeed):
+    return PACKET(MOTOR_REQUEST, DATA(motor=MOTOR(
+        (LEFT_REVERSE if leftSpeed < 0 else 0x00)
+            | (RIGHT_REVERSE if rightSpeed < 0 else 0x00),
+        min(abs(leftSpeed), 255),
+        min(abs(rightSpeed), 255)
+    )))
+
+def timing_request():
+    return PACKET(TIMING_REQUEST)
+
+def voltage_request():
+    return PACKET(VOLTAGE_REQUEST)
+
+def ledon_request():
+    return PACKET(LEDON_REQUEST)
+
+def boostcfg_request(method, threshold=0, val=0, cycles=0):
+    return PACKET(BOOSTCFG_REQUEST, DATA(boost=BOOST(
+        method,
+        threshold,
+        val,
+        cycles
+    )))
+
+def _find_getch():
+    try:
+        import msvcrt
+        return msvcrt.getch
+    except ImportError:
+        def _getch():
+            """
+            Gets a single character from STDIO.
+            """
+            import sys
+            import tty
+            import termios
+            fd = sys.stdin.fileno()
+            old = termios.tcgetattr(fd)
+            try:
+                tty.setraw(fd)
+                return sys.stdin.read(1)
+            finally:
+                termios.tcsetattr(fd, termios.TCSADRAIN, old)
+        return _getch
+
+getch = _find_getch()
+
+IP_PREFIX = "192.168.0."
+PORT = 12344
+
+def loop(id, sock, addr):
+
+    ROBOT_ID = id;
+
+    leftSpeed = 0
+    rightSpeed = 0
+
+    print IP_PREFIX+str(ROBOT_ID)
+
+    while True:
+
+        speedChanged = False
+
+        command = getch()
+
+        if command == 'w':
+            speedChanged = True
+            leftSpeed += 10
+            rightSpeed += 10
+        elif command == 's':
+            speedChanged = True
+            leftSpeed -= 10
+            rightSpeed -= 10
+        elif command == 'a':
+            speedChanged = True
+            leftSpeed -= 5
+            rightSpeed += 5
+        elif command == 'd':
+            speedChanged = True
+            leftSpeed += 5
+            rightSpeed -= 5
+        elif command == 'm':
+            speedChanged = True
+            leftSpeed = 255
+            rightSpeed = 255
+        elif command == 'n':
+            speedChanged = True
+            leftSpeed = 0
+            rightSpeed = 0
+        elif command == 'v':
+            while True:
+                sock.sendto(voltage_request(), (IP_PREFIX+str(ROBOT_ID), PORT));
+                try:
+                    packet = PACKET()
+                    nbytes, new_packet_addr = sock.recvfrom_into(packet)
+
+                    if new_packet_addr[0] == IP_PREFIX+str(ROBOT_ID):
+                        if packet.type == VOLTAGE_REPLY:
+                            print "Voltage: "+packet.data.voltStr
+                            break;
+                        else:
+                            pass
+                    else:
+                        print "Packet from "+new_packet_addr[0]+". Ignoring"
+                except socket.timeout:
+                    return;
+        elif command == 'p':
+            sock.sendto(boostcfg_request(BOOST_PUSH, 8, 35, 20), (IP_PREFIX+str(ROBOT_ID), PORT));
+        elif command == 'i':
+            sock.sendto(boostcfg_request(BOOST_INTERLACE, 16), (IP_PREFIX+str(ROBOT_ID), PORT));
+        elif command == 'o':
+            sock.sendto(boostcfg_request(BOOST_OFF), (IP_PREFIX+str(ROBOT_ID), PORT));
+        elif command == 'e':
+            sock.sendto(boostcfg_request(BOOST_OFF), (IP_PREFIX+str(ROBOT_ID), PORT));
+            sock.sendto(motor_request(0, 0), (IP_PREFIX+str(ROBOT_ID), PORT));
+            return;
+        else:
+            print "Invalid Command"
+            continue;
+
+        if speedChanged:
+            print "speed: leftSpeed: "+str(leftSpeed)+", rightSpeed: "+str(rightSpeed)
+            sock.sendto(motor_request(leftSpeed, rightSpeed), (IP_PREFIX+str(ROBOT_ID), PORT));
+
+
+def main():
+    try:
+        ROBOT_ID = int(input("Enter ROBOT_ID: "))
+    except:
+        print "Could not parse as Integer"
+        return
+
+    sock = socket.socket(socket.AF_INET, # Internet
+                     socket.SOCK_DGRAM) # UDP
+    sock.bind(("", PORT))
+    sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+    sock.settimeout(5.0)
+
+    print "Searching for Robot "+str(ROBOT_ID)
+    while True:
+        sock.sendto(discovery_request(), (IP_PREFIX+str(ROBOT_ID), PORT));
+        try:
+            packet = PACKET()
+            nbytes, addr = sock.recvfrom_into(packet)
+
+            if packet.type == DISCOVERY_REPLY:
+                if packet.data.robotID == ROBOT_ID:
+                    sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 0)
+
+                    print "Connected"
+                    print ""
+                    print "Commands:"
+                    print " w: Increase Speed"
+                    print " s: Decrease Speed"
+                    print " a: Increase Left Turn"
+                    print " d: Increase Right Turn"
+                    print " m: Max Speed"
+                    print " n: No Speed"
+                    print " v: Voltage Request"
+                    print " p: Boost Push"
+                    print " i: Boost Interlaced"
+                    print " o: Boost Off"
+                    print " e: Exit"
+                    print ""
+
+                    # not working right now
+                    #sock.sendto(ledon_request(), (IP_PREFIX+str(ROBOT_ID), PORT));
+                   
+                    # used for quick distance measuring
+                    #print "Start" 
+                    #sock.sendto(motor_request(255,255), (IP_PREFIX+str(ROBOT_ID), PORT));
+                    #time.sleep(1.0)
+                    #sock.sendto(motor_request(0, 0), (IP_PREFIX+str(ROBOT_ID), PORT));
+                    #print "Stop"
+                    
+                    loop(ROBOT_ID, sock, addr)
+
+                    print ""
+                    print "Disconnected"
+                    return
+
+                else:
+                    print "Discovered Robot "+str(robot.data_robotID)+". Ignoring"
+            else:
+                pass
+        except socket.timeout:
+            pass
+        time.sleep(5.0)
+
+if __name__ == "__main__":
+    main()