odekolon писал(а):Как говориться "из говна и палок"!
Совершенно верно!
Задача стояла построить, чтоб работало! И, работает!
odekolon писал(а):как на ардуине умудрились сервопетлю с энкодерами, по постоянному току сделать
Над этим долго голову ломал. Было прочитано довольно много инфы, и в итоге получилось.
Сигналы с энкодеров идут на внешние прерывания ардуины (У Mega прерываний 6, потому и была выбрана именно она).
Код ардуины для работы с энкодероми и моторами получился такой:
Код: Выделить всё
void setup() {
motor1.setSpeed(255);
motor2.setSpeed(255);
motor3.setSpeed(255);
int i;
Serial.begin(9600);
attachInterrupt(3, blink1, CHANGE);
attachInterrupt(2, blink2, CHANGE);
attachInterrupt(4, blink3, CHANGE);
delayTime=4; //задержка между шагами 3 мс (2 мс - время реакции ШД на подачу напряжения)
}
void moveBackward(int sm)
{
if (sm == 0)
{
while (P1 < 1){
motor1.run(FORWARD);
}
motor1.run(RELEASE);
P1 = 0;
}
else if (sm == 1)
{
while (P2 < 1){
motor2.run(FORWARD);
}
motor2.run(RELEASE);
P2 = 0;
}
else if (sm == 2)
{
while (P3 < 1){
motor3.run(FORWARD);
}
motor3.run(RELEASE);
motor3.run(BACKWARD);
delay(5);
motor3.run(RELEASE);
P3 = 0;
}
}
void moveForward(int sm)
{
if (sm == 0)
{
while (P1 < 1){
motor1.run(BACKWARD);
}
motor1.run(RELEASE);
P1 = 0;
}
else if (sm == 1)
{
while (P2 < 1){
motor2.run(BACKWARD);
}
motor2.run(RELEASE);
P2 = 0;
}
else if (sm == 2)
{
while (P3 < 1){
motor3.run(BACKWARD);
}
motor3.run(RELEASE);
motor3.run(FORWARD);
delay(5);
motor3.run(RELEASE);
P3 = 0;
}
}
Полный код:
Код: Выделить всё
#include <AFMotor.h>
int motorPins[3][4] = {{2, 3, 4, 5},{7, 8, 6, 9},{10, 11, 12, 13}};
int count;
int count2[3] = {0,0,0};
int delayTime;
int val = 0;
int rot=0;
int incomingByte = 0;
int sign=1;
int P1,P2,P3, s = 0;
AF_DCMotor motor1(3, MOTOR12_64KHZ);
AF_DCMotor motor2(2, MOTOR12_64KHZ);
AF_DCMotor motor3(4, MOTOR12_64KHZ);
void blink1()
{
P1++;
}
void blink2()
{
P2++;
}
void blink3()
{
P3++;
}
//Процедура настройки прошивки
void setup() {
motor1.setSpeed(255);
motor2.setSpeed(255);
motor3.setSpeed(255);
int i;
Serial.begin(9600);
attachInterrupt(3, blink1, CHANGE);
attachInterrupt(2, blink2, CHANGE);
attachInterrupt(4, blink3, CHANGE);
delayTime=4; //задержка между шагами 3 мс (2 мс - время реакции ШД на подачу напряжения)
}
void moveBackward(int sm)
{
if (sm == 0)
{
while (P1 < 1){
motor1.run(FORWARD);
}
motor1.run(RELEASE);
P1 = 0;
}
else if (sm == 1)
{
while (P2 < 1){
motor2.run(FORWARD);
}
motor2.run(RELEASE);
P2 = 0;
}
else if (sm == 2)
{
while (P3 < 1){
motor3.run(FORWARD);
}
motor3.run(RELEASE);
motor3.run(BACKWARD);
delay(5);
motor3.run(RELEASE);
P3 = 0;
}
}
void moveForward(int sm)
{
if (sm == 0)
{
while (P1 < 1){
motor1.run(BACKWARD);
}
motor1.run(RELEASE);
P1 = 0;
}
else if (sm == 1)
{
while (P2 < 1){
motor2.run(BACKWARD);
}
motor2.run(RELEASE);
P2 = 0;
}
else if (sm == 2)
{
while (P3 < 1){
motor3.run(BACKWARD);
}
motor3.run(RELEASE);
motor3.run(FORWARD);
delay(5);
motor3.run(RELEASE);
P3 = 0;
}
}
//Одновременный поворот двигателей 0, 1, 2 на x, y, z шагов соответственно
void MoveSM(long x, long y, long z) {
long c[3], c2[3];
double c1[3], d[3];
long m, i;
boolean flg;
c[0] = x;
c[1] = y;
c[2] = z;
m = 1;
for (i=0; i<3; i++) {
if (m < abs(c[i])) m = abs(c[i]);
}
for (i=0; i<3; i++) {
c1[i] = 0;
d[i] = 1.0 * c[i] / m;
c2[i] = 0;
}
flg = false;
for (i=0; i<3; i++) {
if (abs(c1[i]) < abs(c[i])) flg=true;
}
while (flg) {
flg=false;
for (i=0; i<3; i++) {
if (abs(c1[i]) < abs(c[i]))
c1[i] += d[i];
if (abs(c1[i]) - abs(c2[i]) >= 0.5) {
if (c[i]>0) {
c2[i]++;
moveForward(i);
} else {
c2[i]--;
moveBackward(i);
}
}
if (abs(c1[i]) < abs(c[i])) flg=true;
}
delay(delayTime);
}
}
//Основной цикл
void loop() {
if (Serial.available() > 0) { //Пришла команда
long c[4]={0,0,0,0};
int i;
sign=1;
i=0;
incomingByte = Serial.read();
while (incomingByte!=';') { //Читаем входящую строку, признак конца строки знак "точка с запятой"
if (c[i]==0) {
if (incomingByte=='-')
sign=-1;
}
if (incomingByte==',') {
c[i]*=sign;
sign=1;
i++;
} else if (incomingByte>='0' && incomingByte<='9') {
c[i]=c[i]*10+incomingByte-'0';
}
while (Serial.available() == 0) {
delay(2);
}
incomingByte = Serial.read();
}
c[i]*=sign;
MoveSM(c[0],c[1],c[2]); //Вращаем двигатели на заданное число шагов
Serial.println("OK"); //Отправляем компьютеру сообщение "OK", значит можно высылать новую команду
}
else
delay(100); //Если ничего не пришло, ждём 1/10 секуды.
}
Сами энкодеры простые, 4 пин, направление не считывают.
