I tried ServoTimer2 library and the example code works ok. I'm trying it out on arduino mega.
I used the servotimer2 in my code and one of the servo works just like intended. I'm able to pan the servo or if I remove pan servo and put in the tilt servo it works fine too.
The problem is that if I attach two servos then the first attached servo goes wild. Goes to 180 degrees and stays there and no further code executes. But if I remove one servo and only attach one servo then it works flawlessly. I'm confused as to why servotimer2 example code can run two servos but it fails to do it in my code. I was using pin2 and 3 then I thought pin2 might be an interrupt somewhere and used pin4 like in the example but still servo goes wild if I attach both servos.
Also, if someone can help me split the received string, it will reduce the delay I have in my logic.
String I receive is in this format: Say, 127,127,0,0 (Pot1, Pot2, Zbutton, Cbutton). wiht comma as delimiter.
My code:
//#include <Servo.h>
#include <ServoTimer2.h> // the servo library
// Include RadioHead Amplitude Shift Keying Library
#include <RH_ASK.h>
// Include dependant SPI Library
#include <SPI.h>
//Right_Wheel
#define in1 9
#define in2 10
//Left_Wheel
#define in3 7
#define in4 5
// Define output strings
String str_pan;
String str_tilt;
//String str_out;
String str_zButton;
String str_cButton;
String dummy;
int pan = 1500; // 750-2250 microseconds
int tilt = 1500;
int zButton;
int cButton;
int x = 0;
// Create Amplitude Shift Keying Object
RH_ASK rf_driver;
ServoTimer2 panServo;
ServoTimer2 tiltServo;
void setup()
{
// Initialize ASK Object
rf_driver.init();
// Setup Serial Monitor
Serial.begin(9600);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
panServo.attach(4);
//tiltServo.attach(3);
panServo.write(pan);
//tiltServo.write(tilt);
}
void loop()
{
// Set buffer to size of expected message
//Assingned 0s to fix the issue that caused buf to have garbage data
//If buf is declared and not initialized then causes garbage data at the end
uint8_t buf[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0 , 0, 0};
uint8_t buflen = sizeof(buf);
char* name = NULL;
// Check if received packet is correct size
if (rf_driver.recv(buf, &buflen))
{
// Message received with valid checksum
// Get values from string
// Convert received data into string
//str_out = String((char*)buf);
name = strtok(buf, ",");
++x;
while (name != NULL)
{
if (x == 1) {
dummy = String(name);
pan = dummy.toInt();
Serial.println(pan);
}
if (x == 2) {
dummy = String(name);
tilt = dummy.toInt();
Serial.println(tilt);
}
if (x == 3) {
dummy = String(name);
zButton = dummy.toInt();
Serial.println(zButton);
}
if (x == 4) {
dummy = String(name);
cButton = dummy.toInt();
Serial.println(cButton);
}
name = strtok(NULL, ",");
++x;
}
if (zButton == 1) {
if (tilt > 150) {
if (pan > 80 && pan < 150) {
forward();
}
}
else if (tilt < 80) {
if (pan > 80 && pan < 150) {
reverse();
}
}
else if (pan > 150) {
if (tilt > 80 && tilt < 150) {
right();
}
}
else if (pan < 80) {
if (tilt > 80 && tilt < 150) {
left();
}
}
else{
stops();
}
}
else if (cButton == 1) {
pan = map(pan, 27, 230, 750, 2250);
panServo.write(pan);
tilt = map(tilt, 31, 223, 750, 2250);
tiltServo.write(tilt);
}
else{
stops();
}
x = 0;
}
}
void forward() {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
void reverse() {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
void right() {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
void left() {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
void stops() {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}