The "solid model" was made in the program SolidWorks. You can drag things around pivots within the program.
This is the current code I'm working with. It has comments, but I'm still trying to really understand what's going on. (I got this from someone who helps me with programming.) And the video link below that of first the code running an HS56 servo, then one axis (Elevation) connected on my hardware - you can see it needs some mechanical damping, but I know how to do that.
-------------
#include <Servo.h>
#define NUMPOINTS 10 //Number of points to navigate to
#define AZ 0 //Where in the point the azimuth is stored
#define EL 1 //Where in the point the elevation is stored
#define TIME 2 //Where in the point the time is stored
Servo azServo; //Create a class instance to hold the azimuth servo
Servo elServo; //Create a class instance to hold the elevation servo
const int points[NUMPOINTS][3] = { //put the data into the points array
{800, 1850, 500}, //declared as const int so it is stored in program memory as opposed to ram
{840, 1600, 500},
{860, 1350, 500},
{900, 1321, 500},
{1000, 1290, 400},
{1100, 1330, 400},
{1150, 1400, 400},
{1150, 1350, 400},
{1250, 1321, 400},
{1300, 1290, 400},
};
void updateServos();
void printMillis();
//runs once at the begining of the program
void setup()
{
azServo.attach(9); //attach the servo on pin 9 to the azimuth instance.
elServo.attach(10); //attach the servo on pin 10 to the elevation instance.
Serial.begin(9600); //open the serial port at 9600 bps:
}
//main program loop
void loop()
{
updateServos();
printMillis();
}
//Updates both servos to the current point
void updateServos()
{
static unsigned long timer = 0; //timer variable
static int currentPoint = 0; //The point that the servos will be set to
if(millis() > timer) //Check if its time to update the servos
{
azServo.writeMicroseconds(points[currentPoint][AZ]); //set the asimuth servo. the writeMicroseconds function lets you control the pulse instead of setting the degrees
elServo.writeMicroseconds(points[currentPoint][EL]); //set the elevation servo.
timer = millis() + points[currentPoint][TIME]; //set the timer to however long the point specifies
currentPoint++; //increments to the next point in the list
if(currentPoint >= NUMPOINTS) //if points have run out rest to the first one
{
currentPoint = 0;
}
}
}
void printMillis()
{
static unsigned long timer =0;
if(millis()>timer)
{
Serial.print(millis());
Serial.print("\n");
timer = millis()+500;
}
}
-------------------