Full Tripod Controller code

/** This code is version 01 of sunCam The code below takes 2 int's serial A and B A controls the angle of the rotation of 360 B controls the pitch angle of a servo up to 180 pins: 2: is white rotary encoder 3: is brown rotary encoder 8: is to H-Bridge motor 9: is to H-Bridge motor 10: is to servo Attach the rotary encoder and servo + and - to arduino **/ int encoder0PinA = 2; int encoder0PinB = 3; int servoPin = 10; //pin for servo pwm int motorPin0 = 8; int motorPin1 = 9; //variables char serStringA[5]; char serStringB[5]; int serIntA; int serIntB; int oldRot = 500; int angle = 0; int angleB = 0; volatile int encoder0Pos = 0; boolean move = true; boolean moveB = true; boolean extended = false; boolean active = true; boolean hide = false; int minPulse = 910; // Minimum servo position int maxPulse = 2500; // Maximum servo position // Amount to pulse the servo void setup() { Serial.begin(9600); pinMode(servoPin, OUTPUT); digitalWrite(servoPin, LOW); // Turn the motor off pinMode(encoder0PinA, INPUT); digitalWrite(encoder0PinA, HIGH); pinMode(encoder0PinB, INPUT); digitalWrite(encoder0PinB, HIGH); attachInterrupt(0, doEncoderA, CHANGE); // using interrupt to call doEncoder function attachInterrupt(1, doEncoderB, CHANGE); pinMode(motorPin0, OUTPUT); pinMode(motorPin1, OUTPUT); delay(100); } void loop(){ //gives number between 0 - 256 (encoder res 4096) / 16 //sort out horizontal rotation if (encoder0Pos < angle && move == true){ anticlockwise(6); } if (encoder0Pos > angle && move == true){ clockwise(6); } if (encoder0Pos == angle && move == true){ move = false; serialWrite('C'); // Identifier for serial reading // anything printed to serial will go to PC printInteger(encoder0Pos); serialWrite(10); } if (Serial.available()){ readSerial(); } delay(50); if (serIntA > 0){ Serial.print('A'); Serial.println(serIntA); angle = serIntA; serIntA = 0; clearSerial(); move = true; } if (serIntB > 0){ Serial.print('B'); Serial.println(serIntB); angleB = serIntB; serIntB = 0; clearSerial(); moveB = true; servoMotion(angleB); } } void readSerial() { //char *strArray; char channel = Serial.read(); int i = 0; if(!Serial.available()) { return; } if (channel == 'A'){ Serial.println('A'); while (Serial.available()) { serStringA[i] = Serial.read(); i++; } serIntA = atoi(serStringA); } // Serial.println("foof"); if (channel == 'B'){ Serial.println('B'); while (Serial.available()) { serStringB[i] = Serial.read(); i++; } serIntB = atoi(serStringB); } } void clearSerial() { for (int i=0; i < 5; i++) { serStringA[i] = ' '; serStringB[i] = ' '; } } void servoMotion(int rot){ boolean forward; while (true){ if (rot < 500 || rot > 2500){ break; } digitalWrite(servoPin, HIGH); // Turn the motor on delayMicroseconds(oldRot); // Length of the pulse sets the motor position digitalWrite(servoPin, LOW); // Turn the motor off if (oldRot > rot){ //1840 forward = false; } if (oldRot < rot){ //910 forward = true; } if (forward == true){ oldRot ++; }else{ oldRot --; } delay(10); Serial.println(oldRot); if (oldRot == rot){ break; } } } void clockwise(int i){ digitalWrite(motorPin0, HIGH); digitalWrite(motorPin1, LOW); delay(i); digitalWrite(motorPin0, LOW); digitalWrite(motorPin1, LOW); delay (i); } void anticlockwise(int i){ digitalWrite(motorPin0, LOW); digitalWrite(motorPin1, HIGH); delay(i); digitalWrite(motorPin0, LOW); digitalWrite(motorPin1, LOW); delay(i); } void doEncoderA(){ if (digitalRead(encoder0PinA) == HIGH) { if (digitalRead(encoder0PinB) == LOW) { encoder0Pos = encoder0Pos - 1; } else { encoder0Pos = encoder0Pos + 1; } } else { if (digitalRead(encoder0PinB) == LOW) { encoder0Pos = encoder0Pos + 1; } else { encoder0Pos = encoder0Pos - 1; } } } void doEncoderB(){ if (digitalRead(encoder0PinB) == HIGH) { if (digitalRead(encoder0PinA) == LOW) { encoder0Pos = encoder0Pos + 1; } else { encoder0Pos = encoder0Pos - 1; } } else { if (digitalRead(encoder0PinA) == LOW) { encoder0Pos = encoder0Pos - 1; } else { encoder0Pos = encoder0Pos + 1; } } }