daily pastebin goal
10%
SHARE
TWEET

Untitled

a guest Sep 22nd, 2017 45 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define QUAD_A_PIN            2
  2. #define QUAD_B_PIN            3
  3. #define DEBUG_PIN             4
  4. #define MOTOR_SPEED_PIN       5
  5. #define MOTOR_DIR_PIN         6
  6. #define MOTOR_SLEEP_PIN       7
  7. #define MOTOR_MODE_PIN        8
  8. #define STEP_PIN              11
  9. #define DIR_PIN               12
  10. #define ENABLE_PIN            13
  11.  
  12. volatile int position;
  13. int target;
  14.  
  15. char incoming; // incoming serial data
  16. char cmd_arr[80]={0};
  17. int _i=0;
  18.  
  19. bool newStep = false;
  20. bool newEnable = false;
  21. bool oldStep = false;
  22. bool oldEnable = true;
  23. bool dir = false;
  24. bool olddir = false;
  25.  
  26. void setup()
  27. {
  28.     Serial.begin(115200);
  29.    
  30.     while(!Serial.available()){
  31.       Serial.println("A");
  32.       __builtin_avr_delay_cycles(10000000);
  33.     }
  34.     Serial.flush();
  35.    
  36.     Serial.println("MakerBot DC Servo Controller v1.0");
  37.  
  38.     pinMode(QUAD_A_PIN, INPUT);
  39.     pinMode(QUAD_B_PIN, INPUT);
  40.  
  41.     pinMode(DEBUG_PIN, OUTPUT);
  42.  
  43.     pinMode(MOTOR_SLEEP_PIN, OUTPUT);
  44.     digitalWrite(MOTOR_SLEEP_PIN, LOW);
  45.  
  46.     pinMode(MOTOR_MODE_PIN, OUTPUT);
  47.     digitalWrite(MOTOR_MODE_PIN, LOW);
  48.  
  49.     pinMode(MOTOR_SPEED_PIN, OUTPUT);
  50.     digitalWrite(MOTOR_SPEED_PIN, LOW);
  51.  
  52.     pinMode(MOTOR_DIR_PIN, OUTPUT);
  53.     digitalWrite(MOTOR_DIR_PIN, LOW);
  54.  
  55.     pinMode(STEP_PIN, INPUT);
  56.     pinMode(DIR_PIN, INPUT);
  57.     pinMode(ENABLE_PIN, INPUT);
  58.  
  59.     attachInterrupt(0, read_quadrature_a, CHANGE);
  60.     attachInterrupt(1, read_quadrature_b, CHANGE);
  61. }
  62.  
  63. void loop()
  64. {
  65.     newEnable = digitalRead(ENABLE_PIN);
  66.     newStep = digitalRead(STEP_PIN);
  67.     dir = digitalRead(DIR_PIN);
  68.     byte motor_speed = 0;
  69.  
  70.  
  71.     if (Serial.available()) {
  72.         incoming = Serial.read();  
  73.         if (incoming == 's'){  //step
  74.             newStep ^= 1;
  75.             Serial.flush();
  76.             Serial.print("newStep=");
  77.             Serial.println(newStep,DEC);
  78.         }else if (incoming == 'e'){  //enable
  79.             newEnable ^= 1;
  80.             Serial.flush();
  81.             Serial.print("newEnable=");
  82.             Serial.println(newEnable,DEC);
  83.         }else if (incoming == 'a'){  //direction
  84.             digitalWrite(MOTOR_DIR_PIN, HIGH);
  85.             Serial.flush();
  86.             Serial.println("Dir=1");
  87.         }else if (incoming == 'd') { //direction
  88.             digitalWrite(MOTOR_DIR_PIN, LOW);
  89.             Serial.flush();
  90.             Serial.println("Dir=0");
  91.         }else if (incoming == '\n' || incoming == '\r'){ //enter
  92.             cmd_arr[_i]=0;
  93.             motor_speed=atoi(cmd_arr);
  94.             analogWrite(MOTOR_SPEED_PIN, motor_speed);
  95.             Serial.flush();
  96.                         Serial.println();
  97.             Serial.print("motor_speed=");
  98.             Serial.println(motor_speed,DEC);
  99.             _i=0;
  100.             cmd_arr[_i]=0;
  101.         }else if ((incoming > 48) or (incoming < 57)){ //numbers only
  102.             cmd_arr[_i]=incoming; //copy the serial byte to the array
  103.             _i++; // increase the array index
  104.             Serial.flush();
  105.             //Serial.print(_i);
  106.             Serial.print(incoming);
  107.         }
  108.     }
  109.      
  110.  
  111.     //low to high transition
  112.     if (!oldEnable && newEnable)
  113.     {
  114.         //enable is active low, so disable.
  115.         digitalWrite(MOTOR_SLEEP_PIN, LOW);
  116.         digitalWrite(MOTOR_SPEED_PIN, LOW);
  117.     }
  118.     // high to low transition
  119.     else if (oldEnable && !newEnable)
  120.     {
  121.         //enable is active low, so enable.
  122.         digitalWrite(MOTOR_SLEEP_PIN, HIGH);
  123.         delay(1); //give it a millisecond to turn on.
  124.     }
  125.  
  126.     //enable is active low, so only do this if we're enabled.
  127.     if (!newEnable)
  128.     {
  129.    
  130.         if (olddir != dir) {
  131.           target=0;
  132.           position=0;
  133.       olddir = dir;
  134.         }
  135.  
  136.         // step signal is on the low to high transition.
  137.         if (!oldStep && newStep)
  138.         {
  139.             if (dir)
  140.                 target++;
  141.             else
  142.                 target--;
  143.         }
  144.  
  145.                 motor_speed = 0;
  146.                 int distance = abs(position-target);
  147.  
  148.                 if (distance > 255)
  149.                   motor_speed = 255;
  150.                 else
  151.                   motor_speed = distance;
  152.  
  153.         //super primitive control of the motor.
  154.         if (position > target)
  155.         {
  156.             digitalWrite(MOTOR_DIR_PIN, HIGH);
  157.             analogWrite(MOTOR_SPEED_PIN, motor_speed);
  158.         }
  159.         else if (position < target)
  160.         {
  161.             digitalWrite(MOTOR_DIR_PIN, LOW);
  162.             analogWrite(MOTOR_SPEED_PIN, motor_speed);
  163.         }
  164.         else
  165.         {
  166.             digitalWrite(MOTOR_SPEED_PIN, LOW);
  167.         }
  168.  
  169.         oldStep = newStep;
  170.     }
  171.  
  172.     oldEnable = newEnable;
  173.  
  174.         /*
  175.           delay(1000);
  176.           Serial.print("Pos:");
  177.           Serial.println(position, DEC);
  178.         */
  179. }
  180.  
  181. void read_quadrature_a()
  182. {
  183.     // found a low-to-high on channel A
  184.     if (digitalRead(QUAD_A_PIN) == HIGH)
  185.     {
  186.         // check channel B to see which way
  187.         if (digitalRead(QUAD_B_PIN) == LOW)
  188.             position--;
  189.         else
  190.             position++;
  191.     } // found a high-to-low on channel A
  192.     else
  193.     { // check channel B to see which way
  194.          if (digitalRead(QUAD_B_PIN) == LOW)
  195.              position++;
  196.          else
  197.             position--;
  198.     }
  199. }
  200.  
  201. void read_quadrature_b()
  202. {
  203.     // found a low-to-high on channel A
  204.     if (digitalRead(QUAD_B_PIN) == HIGH)
  205.     {
  206.         // check channel B to see which way
  207.         if (digitalRead(QUAD_A_PIN) == LOW)
  208.             position++;
  209.         else
  210.             position--;
  211.     } // found a high-to-low on channel A
  212.     else
  213.     { // check channel B to see which way
  214.          if (digitalRead(QUAD_A_PIN) == LOW)
  215.              position--;
  216.          else
  217.             position++;
  218.     }
  219. }
RAW Paste Data
Top