I have a ServoCity Pan and Tilt mount with two HS 785 servos (one each for pan and tilt) to operate a video camera. I've built a box that has 8 buttons and a "program" button. My goal is to move the pan/tilt (using rotary encoders) to a position I want to save, press the Program button then one of the 8 "memory" buttons so I can recall the location by pressing the corresponding memory button. A rotary potentiometer sets the speed that it moves form the current position to the memorized position.
The trick is, I want the pan movement and tilt movement to end at the same time and at a rate determined by the speed potentiometer. I do this in my code by determining which direction it needs to go for both pan and tilt (in positive or negative microseconds) then which value is greater. The greater movement (pan or tilt) takes one step for each factional step taken for the lesser movement. This necessitates float type numbers.
I'm new to C and Arduino but not to programming. I'd love for one of you fine folks who are much more knowledgeable than I to look at my code and tell me how to make it more efficient. It seems a bit clunky to me.
At the heart is an Arduino Mega 2560. The buttons have LEDs that show the last one selected and, in program mode, the "empty" memory locations.
The code for the rotary encoders were ripped form these pages. Thank you for that and for what I'm about to learn.
#define encoder0PinA 2 //Pan
#define encoder0PinB 3
#define encoder1PinA 18 //Tilt
#define encoder1PinB 19
int ProgCmd = 23;
int ProgLED = 27;
int MemCmd[]={31,32,33,34,35,36,37,38};
int MemLED[]={41,42,43,44,45,46,47,48};
int SpeedPin = 0; //analog pin for speed pot
int LastMemCmd = -1;
#include <Servo.h>
Servo Servo_Pan;
Servo Servo_Tilt;
int ProgMode = false;
int Speed; //sets speed for programmed positions; lower number, less delay
volatile int Rval_Pan = 0;
volatile int Pos_Pan=1500;
volatile int Rval_Tilt = 0;
volatile int Pos_Tilt =1500;
float fltVal;
int OrigVal;
int MemPan[]{0,0,0,0,0,0,0,0};
int MemTilt[]{0,0,0,0,0,0,0,0};
int PanMove;
int TiltMove;
int PanSign;
int TiltSign;
int MaxMove;
int MidServo = 1500;
int DivAmtTilt = 20; //Sensitivity: The lower the number the less turns reqd for movement.
int DivAmtPan = 10; //Sensitivity: The lower the number the less turns reqd for movement.
int Last_Pos_Pan;
int Last_Pos_Tilt;
void setup() {
Servo_Pan.attach(4, 600, 2400); //the pin for the servo control
Servo_Tilt.attach(5, 600, 2400); //the pin for the servo control
pinMode(encoder0PinA, INPUT_PULLUP);
pinMode(encoder0PinB, INPUT_PULLUP);
pinMode(encoder1PinA, INPUT_PULLUP);
pinMode(encoder1PinB, INPUT_PULLUP);
void doEncoder0A();
void doEncoder0B();
void doEncoder1A();
void doEncoder1B();
for (int x=0;x<8;x++){
pinMode(MemCmd[x],INPUT_PULLUP);
pinMode(MemLED[x],OUTPUT);
}
pinMode(ProgCmd, INPUT_PULLUP);
pinMode(ProgLED, OUTPUT);
// encoder pin on interrupt 0 (pin 2)
attachInterrupt(0, doEncoder0A, CHANGE);
// encoder pin on interrupt 1 (pin 3)
attachInterrupt(1, doEncoder0B, CHANGE);
// encoder pin on interrupt 5 (pin 18)
attachInterrupt(5, doEncoder1A, CHANGE);
// encoder pin on interrupt 4 (pin 19)
attachInterrupt(4, doEncoder1B, CHANGE);
Servo_Pan.writeMicroseconds(Pos_Pan);
Servo_Tilt.writeMicroseconds(Pos_Tilt);
Serial.begin (9600); //just for debugging
}
void loop(){
Last_Pos_Pan = Rval_Pan;
Last_Pos_Tilt = Rval_Tilt;
if (Last_Pos_Pan != Rval_Pan) {
Pos_Pan = (Rval_Pan/DivAmtPan) + MidServo;
Servo_Pan.writeMicroseconds(Pos_Pan);
Serial.println(Pos_Pan);
}
if (Last_Pos_Tilt != Rval_Tilt) {
Pos_Tilt = (Rval_Tilt/DivAmtTilt) + MidServo;
Servo_Tilt.writeMicroseconds(Pos_Tilt);
Serial.println(Pos_Tilt);
}
if (digitalRead(ProgCmd) == LOW){
ProgMode = not ProgMode;
if(ProgMode==true){
digitalWrite(ProgLED,HIGH);
SetCmdLED();
}
else {
digitalWrite(ProgLED,LOW);
SetCmdLED();
}
delay(500);
}
for (int x=0;x<8;x++){
if (digitalRead(MemCmd[x]) == LOW){
if (ProgMode == true){
MemPan[x] = Pos_Pan;
MemTilt[x] = Pos_Tilt;
LastMemCmd = x;
SetCmdLED();
ProgMode = false;
digitalWrite(ProgLED,LOW);
delay(500);
}
else {
LastMemCmd = x;
SetCmdLED();
PanMove = MemPan[x] - Pos_Pan;
TiltMove = MemTilt[x] - Pos_Tilt;
if (PanMove < 0){
PanSign=-1;
}
else {
PanSign=1;
}
PanMove=PanMove * PanSign; //make absolute value
if (TiltMove < 0){
TiltSign=-1;
}
else {
TiltSign=1;
}
TiltMove = TiltMove * TiltSign; //make absolute value
MaxMove = max(PanMove, TiltMove);
if (PanMove > TiltMove){
fltVal = TiltMove / MaxMove;
OrigVal = Pos_Tilt;
for (int x=0;x<MaxMove;x++){
Pos_Pan = Pos_Pan + PanSign;
Pos_Tilt = OrigVal + (x * fltVal * TiltSign);
Speed=analogRead(SpeedPin)/10;
delay (Speed);
Servo_Pan.writeMicroseconds(Pos_Pan);
Servo_Tilt.writeMicroseconds(Pos_Tilt);
}
}
else {
fltVal = PanMove / MaxMove;
OrigVal = Pos_Pan;
for (int x=0;x<MaxMove;x++){
Pos_Tilt = Pos_Tilt + TiltSign;
Pos_Pan = OrigVal + (x * fltVal * PanSign);
Speed=(analogRead(SpeedPin)/10);
delay (Speed);
Servo_Pan.writeMicroseconds(Pos_Pan);
Servo_Tilt.writeMicroseconds(Pos_Tilt);
}
}
}
SetCmdLED();
}
}
}
//==============Pan Encoder============================
void doEncoder0A(){
// look for a low-to-high on channel A
if (digitalRead(encoder0PinA) == HIGH) {
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinB) == LOW) {
Rval_Pan = Rval_Pan - 1; // CW
}
else {
Rval_Pan = Rval_Pan + 1; // CCW
}
}
else // must be a high-to-low edge on channel A
{
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinB) == HIGH) {
Rval_Pan = Rval_Pan - 1; // CW
}
else {
Rval_Pan = Rval_Pan + 1; // CCW
}
}
}
void doEncoder0B(){
// look for a low-to-high on channel B
if (digitalRead(encoder0PinB) == HIGH) {
// check channel A to see which way encoder is turning
if (digitalRead(encoder0PinA) == HIGH) {
Rval_Pan = Rval_Pan - 1; // CW
}
else {
Rval_Pan = Rval_Pan + 1; // CCW
}
}
// Look for a high-to-low on channel B
else {
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinA) == LOW) {
Rval_Pan = Rval_Pan + 1; // CW
}
else {
Rval_Pan = Rval_Pan - 1; // CCW
}
}
}
//==============Tilt Encoder============================
void doEncoder1A(){
// look for a low-to-high on channel A
if (digitalRead(encoder1PinA) == HIGH) {
// check channel B to see which way encoder is turning
if (digitalRead(encoder1PinB) == LOW) {
Rval_Tilt = Rval_Tilt + 1; //
}
else {
Rval_Tilt = Rval_Tilt - 1; //
}
}
else // must be a high-to-low edge on channel A
{
// check channel B to see which way encoder is turning
if (digitalRead(encoder1PinB) == HIGH) {
Rval_Tilt = Rval_Tilt + 1; //
}
else {
Rval_Tilt = Rval_Tilt - 1; //
}
}
}
void doEncoder1B(){
// look for a low-to-high on channel B
if (digitalRead(encoder1PinB) == HIGH) {
// check channel A to see which way encoder is turning
if (digitalRead(encoder1PinA) == HIGH) {
Rval_Tilt = Rval_Tilt + 1; //
}
else {
Rval_Tilt = Rval_Tilt - 1; //
}
}
// Look for a high-to-low on channel B
else {
// check channel B to see which way encoder is turning
if (digitalRead(encoder1PinA) == LOW) {
Rval_Tilt = Rval_Tilt + 1; //
}
else {
Rval_Tilt = Rval_Tilt - 1; //
}
}
}
//================Cmd LED=============
void SetCmdLED(){
//Serial.print("Func: LastMemCmd ");
//Serial.println(LastMemCmd);
if (ProgMode==false){
for (int x=0;x<8;x++){
if (LastMemCmd==x){
digitalWrite(MemLED[x],HIGH);
Serial.print("No prog: HIGH ");
Serial.println(x);
}
else{
digitalWrite(MemLED[x],LOW);
Serial.print("No prog: LOW ");
Serial.println(x);
}
}
}
else {
for (int x=0;x<8;x++){
//Serial.println (MemPan[x]);
if(MemPan[x]>0){
digitalWrite(MemLED[x],LOW);
Serial.print("Prog: LOW ");
Serial.println(x);
}
else{
digitalWrite(MemLED[x],HIGH);
Serial.print("Prog: HIGH ");
Serial.println(x);
}
}
}
}