Giant Servo Arduino Source Code

#include <stdint.h> // defines fixed width integers
const int8_t pot_p = A0;
const int8_t led_p = 13;
const int8_t pwm_p = 11;
const int8_t dir_p = 12;
const int16_t hysteresis = 50; // dont correct until we're off by 50
const int16_t margin = 150; // never get closer than 150 counts from the edges
const int16_t pre_stop = 40; // stop early so we coast to the correct position, depends on the load.

enum{ERROR_S,IDLE_S,LEFT_S,RIGHT_S}; // define constants for each state
char state;


void setup() {
  Serial.begin(115200);
  pinMode(led_p, OUTPUT);
  digitalWrite(pwm_p, 0);
  pinMode(pwm_p, OUTPUT);
  pinMode(dir_p, OUTPUT);
  digitalWrite(led_p,0);
  state = IDLE_S;
}


void loop() {
  static int16_t targ_pos = 512; //target position
  static int16_t targ_pos_temp = 512;
  static int16_t pos;            // actual position
  static int16_t err;          // difference between target and acutal position
  char in_byte; // used for serial receive
  static unsigned long last_print_millis = 0;

  pos = analogRead(pot_p);
  err = pos - targ_pos;

  switch(state){
    case IDLE_S:
      if(err > hysteresis && pos > margin){
        state = LEFT_S;
        digitalWrite(dir_p, 1);// turn motor left
        digitalWrite(pwm_p, 1);
      }
      if(err < -hysteresis && pos < 1023 - margin){
        state = RIGHT_S;
        // turn motor right
        digitalWrite(dir_p, 0);// turn motor left
        digitalWrite(pwm_p, 1);
      }
      break;
    case LEFT_S:
      if(err <  pre_stop){
        state = IDLE_S;
        // stop motor
        digitalWrite(pwm_p, 0);
      }
      break;
    case RIGHT_S:
      if(err > pre_stop){
        state = IDLE_S;
        // stop motor
        digitalWrite(pwm_p, 0);
      }
      break;
    case ERROR_S: default:
      digitalWrite(pwm_p, 0);//set PWM to zero
      while(1){ // blink to indicate error
         digitalWrite(led_p,!digitalRead(led_p)); //invert led state
         delay(100);
      }
      break;
  }

//  if(millis() - last_print_millis > 500){
//    last_print_millis = millis();
//    Serial.print((uint8_t)state);
//    Serial.print(" ");
//    Serial.println(pos);
//  }
if(state == LEFT_S || state == RIGHT_S)
  last_print_millis = millis();
if( millis() - last_print_millis < 500){
    Serial.print((uint8_t)state);
    Serial.print(" ");
    Serial.println(pos);
}



  if(Serial.available()){
    targ_pos_temp = 0;
    delay(100);
    while(Serial.available() > 0) {
      in_byte = Serial.read();
      if(in_byte == '\r'){
        if(targ_pos_temp > margin && targ_pos_temp < 1023 - margin)
          targ_pos = targ_pos_temp;
        else
          Serial.println("Target out of range");
        Serial.print("Target Position: "); Serial.println(targ_pos);
        break;
      }
      if(in_byte > '9') break;
      if(in_byte < '0') break;
      targ_pos_temp *= 10; //shift to the left
      targ_pos_temp += in_byte - '0';
    }
  }
}