Guest User

Giant Servo Arduino Source Code

a guest
Jan 15th, 2021
48
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. }
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×