I haven't worked with this stuff in a while. I do know that I gave up on getting it to work with the command letter in front, like P100. So the way to do it is, for example, 1100S. But my code uses degrees and inputs and figures out those numbers. So I'd never send something like P2100, it might be 110P, for 110 degrees to Pan servo.
I can't tell from your code what is wrong, but this is my latest code and it has worked very well. (you can see the application for it at http://www.thitle.com There is a bunch of code that reads from a TPA81 Thermopile array which you can ignore. Sorry I can't be more help at the moment.
Hopefully something here will click with you. Also, I'd first try to get it working from the serial window of a PC. 9600 works fine. I'd use that. Keep in mind that you can get into timing problems with the Arduino if you deviate from norms.
#include <Servo.h>
#include <Wire.h>
#include <SoftwareSerial.h>
Servo panServo;
Servo tiltServo;
// TPA81
#define rxPin 2 // Pin for rx
#define txPin 3 // Pin for tx
#define address 0x68 // Address of TPA81
#define softReg 0x00 // Byte for software version
#define ambiant 0x01 // Byte for ambiant temperature
int temperature[] = {0,0,0,0,0,0,0,0}; // Array to hold temperature data
float angle = 0; // changed to float for fractional degree rotation
boolean isFraction = false;
// *** PAN SERVO ***
// SERVO VARS 360 degrees GWServo S125 1T/2BB
//const int panMin = 1000; // pulse width for 0 degrees
//const int panMax = 1940; // pulse width for 360 degrees
//const int pcenter = 1000; // using 1000 for center, or 0 degrees, you may want to use something different
// SERVO VARS 180 degrees HITEC HS-311
const int panMin = 900; // pulse width for 0 degrees
const int panMax = 2350; // pulse width for 360 degrees
const int pcenter = 1500; // using 1000 for center, or 0 degrees, you may want to use something different
// *** TILT SERVO
const int tiltMin = 900;
const int tiltMax = 2350;
int servoPanPin = 9; // Control pin for servo motor
int servoTiltPin = 10; // Control pin for servo motor
// Can use this later for moving servo more slowly on big jumps
//int servoPanSavePos = 0;
//int servoTiltSavePos = 0;
void setup() {
Serial.begin(9600); // connect to the serial port
panServo.attach(servoPanPin, panMin, panMax);
tiltServo.attach(servoTiltPin, tiltMin, tiltMax);
// Center PAN
int pulsecenter = map(90,0,180,panMin, panMax); // 180 Servo
panServo.writeMicroseconds(pulsecenter);
// Center TILT
pulsecenter = map(45,0,180,tiltMin, tiltMax); // 180 Servo
tiltServo.writeMicroseconds(pulsecenter);
//INIT TPA81
pinMode(rxPin, INPUT);
pinMode(txPin, OUTPUT);
Wire.begin();
//delay(100); // Wait to make sure everything is powerd up
//int software = getData(softReg); // Get software version
//Serial.print("TPA81 Example V:");
//Serial.println(software); // Print software version to the screen
}
void loop() {
readSerialString(); // read, wait for command
}
void readSerialString () {
if ( Serial.available())
{
char ch = Serial.read();
if(ch >= '0' && ch <= '9') // is ch a number?
{
// we're in integer part of number
if (isFraction == false)
{
angle = angle * 10 + ch - '0'; // yes, accumulate the value
}
// were in fraction, add it
else
{
angle = angle + float((ch - '0') * 0.1);
}
}
else if(ch == '.')
{
isFraction = true; // next number will be fractional
}
else if(ch == 'P'| ch == 'p')
{
int pulse = mapFloat(angle,0,180,panMin, panMax); // 360
panServo.writeMicroseconds(pulse); //
Serial.print("P");
//Serial.println(angle, DEC);
printDouble((double)angle, 1);
angle = 0;
isFraction = false;
}
else if(ch == 'T'| ch == 't')
{
int pulseTilt = mapFloat(angle,0,180,tiltMin, tiltMax);
tiltServo.writeMicroseconds(pulseTilt); //
// tiltServo.write(angle); old, simple way
Serial.print("T");
printDouble((double)angle, 1);
// Serial.println(angle, DEC);
angle = 0;
isFraction = false;
}
else if(ch == 'R'| ch == 'r')
{
//Serial.println("Reading temp...");
for(int i = 0; i < 8; i++)
{ // Loops and stores temperature data in array
temperature[i] = getData(i+2);
}
// This order, sensor horizontal, positioned to left of card
Serial.print(temperature[7]);
Serial.print(" ");
Serial.print(temperature[6]);
Serial.print(" ");
Serial.print(temperature[5]);
Serial.print(" ");
Serial.print(temperature[4]);
Serial.print(" ");
Serial.print(temperature[3]);
Serial.print(" ");
Serial.print(temperature[2]);
Serial.print(" ");
Serial.print(temperature[1]);
Serial.print(" ");
Serial.print(temperature[0]);
Serial.println(" ");
}
}
}
int getData(int reg){ // Function to receive one byte of data from TPA81
Wire.beginTransmission(address); // Begin communication with TPA81
Wire.send(reg); // Send reg to TPA81
Wire.endTransmission();
Wire.requestFrom(address, 1); // Request 1 byte
while(Wire.available() < 1); // Wait for byte to arrive
int data = Wire.receive(); // Get byte
return(data); // return byte
}
// Need this to send servor degrees with fractions, like 10.5 degrees
// Thanks Cees-Willem
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void printDouble( double val, byte precision){
// prints val with number of decimal places determine by precision
// precision is a number from 0 to 6 indicating the desired decimial places
// example: lcdPrintDouble( 3.1415, 2); // prints 3.14 (two decimal places)
if(val < 0.0){
Serial.print('-');
val = -val;
}
Serial.print (int(val)); //prints the int part
if( precision > 0) {
Serial.print("."); // print the decimal point
unsigned long frac;
unsigned long mult = 1;
byte padding = precision -1;
while(precision--)
mult *=10;
if(val >= 0)
frac = (val - int(val)) * mult;
else
frac = (int(val)- val ) * mult;
unsigned long frac1 = frac;
while( frac1 /= 10 )
padding--;
while( padding--)
Serial.print("0");
Serial.println(frac,DEC) ;
}
}