Hey guys,
Ok I'm having a problem with the readString(). For some reason the length of my received string is going nuts on me. I send a string from my Android and those strings are either 2 long, s0, 4 long, cl25, or 5 long, cl100. So I've setup a Serial.println right after I receive from my Android and I get things like 14 long, 29 long it varies what am I missing?
I'm posting my code just to show you what I have now:
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
#include <Bridge.h>
#include <Console.h>
#include <Wire.h>
#include <Servo.h>
#include <YunServer.h>
#include <YunClient.h>
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Select which 'port' M1, M2, M3 or M4. In this case, M1
Adafruit_DCMotor *frontWheels = AFMS.getMotor(2);
// You can also make another motor on port M2
Adafruit_DCMotor *rearWheels = AFMS.getMotor(1);
// Declaring the servo that controls the webcam
Servo servo1;
// The pin that the LED is attached to
const int ledPin = 13;
// We open the socket 5678 to communicate.
YunServer server(5555);
void setup() {
Serial.begin(9600);
// Attach the LED to pin 13 as output
pinMode(ledPin, OUTPUT);
digitalWrite(ledPin, HIGH);
// Bridge startup
Bridge.begin();
// Create with the default frequency 1.6KHz
AFMS.begin();
// Front Wheels initialization
frontWheels->setSpeed(150);
frontWheels->run(FORWARD);
frontWheels->run(RELEASE);
// Rear Wheels initialization
rearWheels->setSpeed(150);
rearWheels->run(FORWARD);
rearWheels->run(RELEASE);
server.noListenOnLocalhost();
server.begin();
}
void loop() {
YunClient client = server.accept();
// There is a new client
if (client) {
client.setTimeout(5);
// Declaration of the different variables
int _speed = 0;
int lastSpeed = 0;
String lastDirection = "";
bool cameraOn = 0;
String command = "none";
digitalWrite(ledPin, LOW);
while (!command.equals("stop")) {
if (client.available() > 0) {
command = client.readString();
command.trim();
String _direction = command.substring(0,1);
String _subDirection = command.substring(1,1);
Serial.println("Command: " + command);
Serial.println("Length: " + (String)command.length());
Serial.println("Direction: " + _direction);
Serial.println("Direction: " + _subDirection);
if (_direction.equals("c")) {
cameraOn = 1;
}
if (cameraOn) {
if (_subDirection.equals("l")) {
_speed = map(command.substring(2, 5).toInt(), 0, 180, 1000, 1500);
} else if (_subDirection.equals("r")) {
_speed = map(command.substring(2, 5).toInt(), 0, 180, 1500, 2000);
}
} else if (_direction.equals("f") || _direction.equals("b")) {
_speed = map(command.substring(2, 5).toInt(), 0, 180, 0, 255);
}
else if (_direction.equals("r") || _direction.equals("l")) {
_speed = map(command.substring(2, 5).toInt(), 0, 180, 130, 255);
}
Serial.println("LastSpeed: " + (String)lastSpeed);
Serial.println("Speed: " + (String)_speed);
if (_speed > 0 && (lastSpeed != _speed || !lastDirection.equals(_direction))) {
lastSpeed = _speed;
lastDirection = _direction;
if (_direction.equals("f")) {
Console.println("In f");
Console.println(_speed);
//rearWheels->run(RELEASE);
rearWheels->setSpeed(_speed);
rearWheels->run(FORWARD);
}
else if (_direction.equals("b")) {
rearWheels->run(RELEASE);
rearWheels->setSpeed(_speed);
rearWheels->run(BACKWARD);
}
else if (_direction.equals("l")) {
if (cameraOn) {
if (!servo1.attached()) {
servo1.attach(10);
}
servo1.writeMicroseconds(_speed);
delay(50);
} else {
//frontWheels->run(RELEASE);
frontWheels->setSpeed(_speed);
frontWheels->run(BACKWARD);
}
}
else if (_direction.equals("r")) {
if (cameraOn) {
if (!servo1.attached()) {
servo1.attach(10);
}
servo1.writeMicroseconds(_speed);
delay(50);
frontWheels->run(RELEASE);
frontWheels->setSpeed(_speed);
frontWheels->run(FORWARD);
}
}
else if (_direction.equals("c")) {
Console.println("In Servo");
}
else if (_direction.equals("d")) {
servo1.writeMicroseconds(1500);
servo1.detach();
}
delay(500);
}
else if (_direction.equals("s")) {
if (!servo1.attached()) {
servo1.attach(10);
servo1.writeMicroseconds(1500);
}
frontWheels->run(RELEASE);
rearWheels->run(RELEASE);
servo1.detach();
}
//Serial.println("Command: " + command);
}
}
digitalWrite(ledPin, HIGH);
if (servo1.attached()) {
servo1.writeMicroseconds(1500);
servo1.detach();
}
client.stop();
}
}