это для репы
Код:
#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
#define MOTOR_START_PWM 107
#define STEP_SIZE 10
volatile int position;
int target;
char incoming; // incoming serial data
char cmd_arr[80]={0};
int _i=0;
bool manual_mode = false;
bool newStep = false;
bool newEnable = false;
bool oldStep = false;
bool oldEnable = true;
bool dir = false;
bool olddir = false;
byte motor_speed = 0;
unsigned char eq=0; // stable position counter
void setup()
{
Serial.begin(38400);
// while(!Serial.available()){
// Serial.println("A");
// __builtin_avr_delay_cycles(10000000);
// }
Serial.flush();
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);
//motor turn on
digitalWrite(MOTOR_SLEEP_PIN, HIGH);
delay(1); //give it a millisecond to turn on.
}
void loop()
{
newEnable = digitalRead(ENABLE_PIN);
newStep = digitalRead(STEP_PIN);
dir = digitalRead(DIR_PIN);
if (Serial.available()) {
incoming = Serial.read();
if (incoming == 's'){ //step
newStep ^= 1;
Serial.flush();
Serial.print("newStep=");
Serial.print(newStep,DEC);
Serial.print(" target=");
Serial.print(target,DEC);
Serial.print(" position=");
Serial.println(position,DEC);
}else if (incoming == 'e'){ //enable
newEnable ^= 1;
Serial.flush();
Serial.print("newEnable=");
Serial.println(newEnable,DEC);
}else if (incoming == 'm'){ //enable
manual_mode ^= 1;
Serial.flush();
Serial.print("manual_mode=");
Serial.println(manual_mode,DEC);
}else if (incoming == 'd') { //direction
dir ^= 1;
Serial.flush();
Serial.print("Dir=");
Serial.println(dir,DEC);
}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.println();
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.print(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)
{
// step signal is on the low to high transition.
if (!oldStep && newStep)
{
eq=0; // reset stable position counter
if (dir)
target+=STEP_SIZE;
else
target-=STEP_SIZE;
}
int position_tmp=position;
if(position_tmp == target){
if(eq>3){ //reset position and target counter
position^=position;
position_tmp^=position_tmp;
target^=target;
}
else eq++;
}else eq=0; // reset stable position counter
if(!manual_mode){
motor_speed = 0;
int distance = abs(position_tmp-target);
if (distance > 255)
motor_speed = 255;
else
motor_speed = MOTOR_START_PWM + distance;
}
//super primitive control of the motor.
/*
//revers of motor
if (position_tmp > 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);
}
*/
if (position_tmp > target)
{
digitalWrite(MOTOR_DIR_PIN, LOW);
analogWrite(MOTOR_SPEED_PIN, motor_speed);
}
else if (position_tmp < target)
{
digitalWrite(MOTOR_DIR_PIN, HIGH);
analogWrite(MOTOR_SPEED_PIN, motor_speed);
}
else
{
digitalWrite(MOTOR_DIR_PIN, HIGH);
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++;
}
}
Добавлено спустя 3 минуты 22 секунды:думаю с кондёром поиграться. он у меня не возле щёток, а в клеммах на плате
Вход
Регистрация
FAQ
Поиск
