#include <math.h>

float degreesPerStep = 0.1758;
float stepsPerDegree = 5.6883;
int count = 0;
const int beeper = 10;
int altHallPin = A6;
int azHallPin = A7;
int laser = 11;
bool laserOn;
int frontRangeAz = 0;
int backRangeAz = 0;
int leftRangeAlt = 0;
int rightRangeAlt = 0;
int frontLeftAlt = 0;
int frontLeftAz = 0;
int frontRightAlt = 0;
int frontRightAz = 0;
int frontCenterAz = 0;
int backLeftAlt = 0;
int backLeftAz = 0;
int backRightAlt = 0;
int backRightAz = 0;
int backCenterAz = 0;
int centerAlt = 0;
int centerMinAlt = 0;
int maxAlt = 0;
int minAlt = 0;
int maxAz = 0;
int minAz = 0;
float maximumSpeedAz = 100.0000;

float computeDegree(int stepPosition) {
  float computedDegree = stepPosition * 0.1758;
  return computedDegree;
}
void saveHighSpeed() {
  //enteredBeep();
  delay(500);
  //nchuk.update();
  for (int i = 20; i < 400; i += 40) {
    float degreeAlt = computeDegree(i);
    Serial.print("Computed Degree = "); Serial.println(degreeAlt);
    degreeAlt = 90.0000 - degreeAlt;
    float distanceCurrent =  24.0000 / tan(degreeAlt * (PI / 180.0000));
    float distPerStepAz = (2.0000 * distanceCurrent * PI) / 2048.0000;
    float maxDistPerSecAz = maximumSpeedAz * .0755;
    float computedSpeedAz = maxDistPerSecAz / distPerStepAz;
    Serial.print("computed Speed Az = "); Serial.println(computedSpeedAz,4);
    delay(2000);
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Setup Started");
}

void loop() {
  // put your main code here, to run repeatedly:
  saveHighSpeed();
}