Advertisement
Guest User

Giant Servo Arduino Source Code

a guest
Jan 15th, 2021
104
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. #include <stdint.h> // defines fixed width integers
  3. const int8_t pot_p = A0;
  4. const int8_t led_p = 13;
  5. const int8_t pwm_p = 11;
  6. const int8_t dir_p = 12;
  7. const int16_t hysteresis = 50; // dont correct until we're off by 50
  8. const int16_t margin = 150; // never get closer than 150 counts from the edges
  9. const int16_t pre_stop = 40; // stop early so we coast to the correct position, depends on the load.
  10.  
  11. enum{ERROR_S,IDLE_S,LEFT_S,RIGHT_S}; // define constants for each state
  12. char state;
  13.  
  14.  
  15. void setup() {                
  16.   Serial.begin(115200);
  17.   pinMode(led_p, OUTPUT);
  18.   digitalWrite(pwm_p, 0);
  19.   pinMode(pwm_p, OUTPUT);
  20.   pinMode(dir_p, OUTPUT);
  21.   digitalWrite(led_p,0);
  22.   state = IDLE_S;
  23. }
  24.  
  25.  
  26. void loop() {
  27.   static int16_t targ_pos = 512; //target position
  28.   static int16_t targ_pos_temp = 512;
  29.   static int16_t pos;            // actual position
  30.   static int16_t err;          // difference between target and acutal position
  31.   char in_byte; // used for serial receive
  32.   static unsigned long last_print_millis = 0;
  33.  
  34.   pos = analogRead(pot_p);
  35.   err = pos - targ_pos;
  36.  
  37.   switch(state){
  38.     case IDLE_S:
  39.       if(err > hysteresis && pos > margin){
  40.         state = LEFT_S;
  41.         digitalWrite(dir_p, 1);// turn motor left
  42.         digitalWrite(pwm_p, 1);
  43.       }
  44.       if(err < -hysteresis && pos < 1023 - margin){
  45.         state = RIGHT_S;
  46.         // turn motor right
  47.         digitalWrite(dir_p, 0);// turn motor left
  48.         digitalWrite(pwm_p, 1);
  49.       }
  50.       break;
  51.     case LEFT_S:
  52.       if(err <  pre_stop){
  53.         state = IDLE_S;
  54.         // stop motor
  55.         digitalWrite(pwm_p, 0);
  56.       }
  57.       break;
  58.     case RIGHT_S:
  59.       if(err > pre_stop){
  60.         state = IDLE_S;
  61.         // stop motor
  62.         digitalWrite(pwm_p, 0);
  63.       }
  64.       break;
  65.     case ERROR_S: default:
  66.       digitalWrite(pwm_p, 0);//set PWM to zero
  67.       while(1){ // blink to indicate error
  68.          digitalWrite(led_p,!digitalRead(led_p)); //invert led state
  69.          delay(100);
  70.       }
  71.       break;
  72.   }
  73.  
  74. //  if(millis() - last_print_millis > 500){
  75. //    last_print_millis = millis();
  76. //    Serial.print((uint8_t)state);
  77. //    Serial.print(" ");
  78. //    Serial.println(pos);
  79. //  }
  80. if(state == LEFT_S || state == RIGHT_S)
  81.   last_print_millis = millis();
  82. if( millis() - last_print_millis < 500){
  83.     Serial.print((uint8_t)state);
  84.     Serial.print(" ");
  85.     Serial.println(pos);
  86. }
  87.  
  88.  
  89.  
  90.   if(Serial.available()){
  91.     targ_pos_temp = 0;
  92.     delay(100);
  93.     while(Serial.available() > 0) {
  94.       in_byte = Serial.read();
  95.       if(in_byte == '\r'){
  96.         if(targ_pos_temp > margin && targ_pos_temp < 1023 - margin)
  97.           targ_pos = targ_pos_temp;
  98.         else
  99.           Serial.println("Target out of range");
  100.         Serial.print("Target Position: "); Serial.println(targ_pos);
  101.         break;
  102.       }
  103.       if(in_byte > '9') break;
  104.       if(in_byte < '0') break;
  105.       targ_pos_temp *= 10; //shift to the left
  106.       targ_pos_temp += in_byte - '0';
  107.     }
  108.   }
  109. }
Advertisement
Advertisement
Advertisement
RAW Paste Data Copied
Advertisement