Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define QUAD_A_PIN 2
- #define QUAD_B_PIN 3
- #define DEBUG_PIN 4
- #define MOTOR_SPEED_PIN 5
- #define MOTOR_DIR_PIN 6
- #define MOTOR_SLEEP_PIN 7
- #define MOTOR_MODE_PIN 8
- #define STEP_PIN 11
- #define DIR_PIN 12
- #define ENABLE_PIN 13
- volatile int position;
- int target;
- char incoming; // incoming serial data
- char cmd_arr[80];
- bool newStep = false;
- bool newEnable = false;
- bool oldStep = false;
- bool oldEnable = true;
- bool dir = false;
- bool olddir = false;
- void setup()
- {
- Serial.begin(19200);
- Serial.println("MakerBot DC Servo Controller v1.0");
- pinMode(QUAD_A_PIN, INPUT);
- pinMode(QUAD_B_PIN, INPUT);
- pinMode(DEBUG_PIN, OUTPUT);
- pinMode(MOTOR_SLEEP_PIN, OUTPUT);
- digitalWrite(MOTOR_SLEEP_PIN, LOW);
- pinMode(MOTOR_MODE_PIN, OUTPUT);
- digitalWrite(MOTOR_MODE_PIN, LOW);
- pinMode(MOTOR_SPEED_PIN, OUTPUT);
- digitalWrite(MOTOR_SPEED_PIN, LOW);
- pinMode(MOTOR_DIR_PIN, OUTPUT);
- digitalWrite(MOTOR_DIR_PIN, LOW);
- pinMode(STEP_PIN, INPUT);
- pinMode(DIR_PIN, INPUT);
- pinMode(ENABLE_PIN, INPUT);
- attachInterrupt(0, read_quadrature_a, CHANGE);
- attachInterrupt(1, read_quadrature_b, CHANGE);
- }
- void loop()
- {
- int i=0;
- newEnable = digitalRead(ENABLE_PIN);
- newStep = digitalRead(STEP_PIN);
- dir = digitalRead(DIR_PIN);
- byte motor_speed = 0;
- if (Serial.available()) {
- incoming = Serial.read();
- if (incoming == 's'){ //step
- newStep ^= newStep;
- Serial.flush();
- Serial.print("newStep=");
- Serial.println(newStep,DEC);
- }else if (incoming == 'e'){ //enable
- newEnable ^= newEnable;
- Serial.flush();
- Serial.print("newEnable=");
- Serial.println(newEnable,DEC);
- }else if (incoming == 'a'){ //direction
- digitalWrite(MOTOR_DIR_PIN, HIGH);
- Serial.flush();
- Serial.println("Dir=1");
- }else if (incoming == 'd') { //direction
- digitalWrite(MOTOR_DIR_PIN, LOW);
- Serial.flush();
- Serial.println("Dir=0");
- }else if (incoming == '\n' || incoming == '\r'){ //enter
- cmd_arr[i]=0;
- motor_speed=atoi(cmd_arr);
- analogWrite(MOTOR_SPEED_PIN, motor_speed);
- Serial.flush();
- Serial.print("motor_speed=");
- Serial.println(motor_speed,DEC);
- i=0;
- cmd_arr[i]=0;
- }else if ((incoming > 48) or (incoming < 57)){ //numbers only
- cmd_arr[i]=incoming; //copy the serial byte to the array
- i++; // increase the array index
- Serial.flush();
- //Serial.print(i);
- //Serial.println(incoming);
- }
- }
- //low to high transition
- if (!oldEnable && newEnable)
- {
- //enable is active low, so disable.
- digitalWrite(MOTOR_SLEEP_PIN, LOW);
- digitalWrite(MOTOR_SPEED_PIN, LOW);
- }
- // high to low transition
- else if (oldEnable && !newEnable)
- {
- //enable is active low, so enable.
- digitalWrite(MOTOR_SLEEP_PIN, HIGH);
- delay(1); //give it a millisecond to turn on.
- }
- //enable is active low, so only do this if we're enabled.
- if (!newEnable)
- {
- if (olddir != dir) {
- target=0;
- position=0;
- olddir = dir;
- }
- // step signal is on the low to high transition.
- if (!oldStep && newStep)
- {
- if (dir)
- target++;
- else
- target--;
- }
- motor_speed = 0;
- int distance = abs(position-target);
- if (distance > 255)
- motor_speed = 255;
- else
- motor_speed = distance;
- //super primitive control of the motor.
- if (position > target)
- {
- digitalWrite(MOTOR_DIR_PIN, HIGH);
- analogWrite(MOTOR_SPEED_PIN, motor_speed);
- }
- else if (position < target)
- {
- digitalWrite(MOTOR_DIR_PIN, LOW);
- analogWrite(MOTOR_SPEED_PIN, motor_speed);
- }
- else
- {
- digitalWrite(MOTOR_SPEED_PIN, LOW);
- }
- oldStep = newStep;
- }
- oldEnable = newEnable;
- /*
- delay(1000);
- Serial.print("Pos:");
- Serial.println(position, DEC);
- */
- }
- void read_quadrature_a()
- {
- // found a low-to-high on channel A
- if (digitalRead(QUAD_A_PIN) == HIGH)
- {
- // check channel B to see which way
- if (digitalRead(QUAD_B_PIN) == LOW)
- position--;
- else
- position++;
- } // found a high-to-low on channel A
- else
- { // check channel B to see which way
- if (digitalRead(QUAD_B_PIN) == LOW)
- position++;
- else
- position--;
- }
- }
- void read_quadrature_b()
- {
- // found a low-to-high on channel A
- if (digitalRead(QUAD_B_PIN) == HIGH)
- {
- // check channel B to see which way
- if (digitalRead(QUAD_A_PIN) == LOW)
- position++;
- else
- position--;
- } // found a high-to-low on channel A
- else
- { // check channel B to see which way
- if (digitalRead(QUAD_A_PIN) == LOW)
- position--;
- else
- position++;
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement