511 lines
13 KiB
Arduino
511 lines
13 KiB
Arduino
|
#include "DualG2HighPowerMotorShield.h"
|
||
|
|
// https://www.pololu.com/docs/0J72
|
||
|
|
// https://github.com/pololu/dual-g2-high-power-motor-shield
|
||
|
|
#include <TimerThree.h>
|
||
|
|
|
||
|
|
// Uncomment the version corresponding with the version of your shield.
|
||
|
|
DualG2HighPowerMotorShield24v14 md;
|
||
|
|
// DualG2HighPowerMotorShield18v18 md;
|
||
|
|
// DualG2HighPowerMotorShield24v18 md;
|
||
|
|
// DualG2HighPowerMotorShield18v22 md;
|
||
|
|
|
||
|
|
//Variables bus RS485
|
||
|
|
const int EnTxPin = 49; // HIGH:TX y LOW:RX
|
||
|
|
const int mydireccion = 108; //Direccion del esclavo
|
||
|
|
#define rs485 Serial2
|
||
|
|
|
||
|
|
#define fc1 18
|
||
|
|
#define fc2 19
|
||
|
|
#define DEBUG false
|
||
|
|
#define MAXSPEED 200
|
||
|
|
#define TIMERAMP 10
|
||
|
|
|
||
|
|
#define N 3 //Numero de vueltas por vuelta
|
||
|
|
int pulses = 0; //Output pulses.
|
||
|
|
uint8_t port = 0;
|
||
|
|
float pulses_max = 2400 * N;
|
||
|
|
|
||
|
|
volatile float angle_final = 0;
|
||
|
|
volatile bool search_angle = false;
|
||
|
|
volatile int speed_actual = 0;
|
||
|
|
volatile bool flag_calibrate = false;
|
||
|
|
volatile bool flag_mov_motor = false;
|
||
|
|
|
||
|
|
unsigned long time_cero = millis();
|
||
|
|
|
||
|
|
void A_CHANGE1() {
|
||
|
|
port = PIND & 0x03; //B00000011;
|
||
|
|
if (port == 0x00) pulses--;
|
||
|
|
else if (port == 0x01) pulses++;
|
||
|
|
else if (port == 0x02) pulses++;
|
||
|
|
else if (port == 0x03) pulses--;
|
||
|
|
}
|
||
|
|
|
||
|
|
void B_CHANGE1() {
|
||
|
|
port = PIND & 0x03; //B00000011;
|
||
|
|
if (port == 0x00) pulses++;
|
||
|
|
else if (port == 0x02) pulses--;
|
||
|
|
else if (port == 0x01) pulses--;
|
||
|
|
else if (port == 0x03) pulses++;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
volatile boolean stop_right = false;
|
||
|
|
volatile boolean flag_right = false;
|
||
|
|
|
||
|
|
void FC1_CHANGE() { //PD3
|
||
|
|
port = PIND & 0x0C; //B00001100;
|
||
|
|
if ((port == 0x00) || (port == 0x04))
|
||
|
|
{
|
||
|
|
if (flag_right)
|
||
|
|
{
|
||
|
|
md.disableDrivers();
|
||
|
|
md.setM1Speed(0);
|
||
|
|
flag_right = false;
|
||
|
|
}
|
||
|
|
stop_right = true;
|
||
|
|
pulses = 0;
|
||
|
|
}
|
||
|
|
else if ((port == 0x0C) || (port == 0x08))
|
||
|
|
{
|
||
|
|
md.enableM1Driver();
|
||
|
|
stop_right = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
volatile boolean stop_left = false;
|
||
|
|
volatile boolean flag_left = false;
|
||
|
|
|
||
|
|
void FC2_CHANGE() { //PD2
|
||
|
|
port = PIND & 0x0C; //B00001100;
|
||
|
|
if ((port == 0x00) || (port == 0x08))
|
||
|
|
{
|
||
|
|
if (flag_left)
|
||
|
|
{
|
||
|
|
md.disableDrivers();
|
||
|
|
md.setM1Speed(0);
|
||
|
|
flag_left = false;
|
||
|
|
}
|
||
|
|
stop_left = true;
|
||
|
|
pulses_max = pulses;
|
||
|
|
}
|
||
|
|
else if ((port == 0x04) || (port == 0x0C))
|
||
|
|
{
|
||
|
|
md.enableM1Driver();
|
||
|
|
stop_left = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void setup()
|
||
|
|
{
|
||
|
|
Serial.begin(115200);
|
||
|
|
#if DEBUG
|
||
|
|
Serial.println("Dual G2 High Power Motor Shield");
|
||
|
|
#endif
|
||
|
|
|
||
|
|
//Inicializacion bus RS485
|
||
|
|
rs485.begin(115200);
|
||
|
|
rs485.setTimeout(100); //establecemos un tiempo de espera de 100ms
|
||
|
|
pinMode(EnTxPin, OUTPUT);
|
||
|
|
digitalWrite(EnTxPin, LOW); //RS485 como receptor
|
||
|
|
|
||
|
|
//Inicializacion de los finales de carrera y parada
|
||
|
|
pinMode(fc1, INPUT);
|
||
|
|
pinMode(fc2, INPUT);
|
||
|
|
if (!digitalRead(fc1)) stop_right = true;
|
||
|
|
if (!digitalRead(fc2)) stop_left = true;
|
||
|
|
|
||
|
|
//Para subir la frecuencia del PWM pin 9 del Mega 2560 a 31333 Hz
|
||
|
|
//TCCR2B = (TCCR2B & 0xF8) | 1 ;
|
||
|
|
|
||
|
|
/* TIMER 2 (Pin 9, 10)
|
||
|
|
Value Divisor Frequency
|
||
|
|
0x01 1 31.374 KHz
|
||
|
|
0x02 8 3.921 KHz
|
||
|
|
0x03 32 980.3 Hz
|
||
|
|
0x04 64 490.1 Hz // default
|
||
|
|
0x05 128 245 hz
|
||
|
|
0x06 256 122.5 hz
|
||
|
|
0x07 1024 30.63 hz*/
|
||
|
|
|
||
|
|
attachInterrupt(3, A_CHANGE1, CHANGE);//PIN20 PD1 Encoder revisar
|
||
|
|
attachInterrupt(2, B_CHANGE1, CHANGE);//PIN21 PD0 Encoder revisar
|
||
|
|
attachInterrupt(5, FC1_CHANGE, CHANGE);//PIN18 PD3 FC1
|
||
|
|
attachInterrupt(4, FC2_CHANGE, CHANGE);//PIN19 PD2 FC2
|
||
|
|
|
||
|
|
md.init();
|
||
|
|
md.calibrateM1CurrentOffset();
|
||
|
|
delay(10);
|
||
|
|
|
||
|
|
Timer3.initialize(10000);
|
||
|
|
Timer3.attachInterrupt(timerRead); // blinkLED to run every 0.15 seconds
|
||
|
|
}
|
||
|
|
|
||
|
|
volatile int speed_max = 0;
|
||
|
|
volatile int angle_mov = 0;
|
||
|
|
|
||
|
|
void rs485read()
|
||
|
|
{
|
||
|
|
if (rs485.available())
|
||
|
|
{
|
||
|
|
if (rs485.read() == 'I') //Si recibimos el inicio de trama
|
||
|
|
{
|
||
|
|
int direccion = rs485.parseInt(); //recibimos la direccion
|
||
|
|
if (direccion == mydireccion) //Si direccion es la nuestra
|
||
|
|
{
|
||
|
|
char funcion = rs485.read(); //leemos el carácter de función
|
||
|
|
|
||
|
|
//---Si el carácter de función es una S entonces la trama es para mover el motor-----------
|
||
|
|
if (funcion == 'S')
|
||
|
|
{
|
||
|
|
angle_mov = rs485.parseInt(); //recibimos el ángulo
|
||
|
|
if (rs485.read() == 'V') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
speed_max = rs485.parseInt(); //recibimos la velocidad
|
||
|
|
|
||
|
|
if (rs485.read() == 'F') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
if (angle_mov < 360) //verificamos que sea un valor en el rango del servo
|
||
|
|
{
|
||
|
|
//myservo.write(angulo); //movemos el servomotor al ángulo correspondiente.
|
||
|
|
flag_mov_motor = true;
|
||
|
|
Serial.print("Slave ");
|
||
|
|
Serial.print(mydireccion);
|
||
|
|
Serial.print(" go to ");
|
||
|
|
Serial.print(angle_mov);
|
||
|
|
Serial.print(" ");
|
||
|
|
Serial.println(speed_max);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (funcion == 'C')
|
||
|
|
{
|
||
|
|
if (rs485.read() == 'F') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
flag_calibrate= true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
//---Si el carácter de función es L entonces el maestro está solicitando una lectura del sensor---
|
||
|
|
else if (funcion == 'L')
|
||
|
|
{
|
||
|
|
if (rs485.read() == 'F') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
//int lectura = analogRead(0); //realizamos la lectura del sensor
|
||
|
|
float temp_angle = actual_angle(); //realizamos la lectura del senso
|
||
|
|
digitalWrite(EnTxPin, HIGH); //rs485 como transmisor
|
||
|
|
rs485.print("i"); //inicio de trama
|
||
|
|
rs485.print(mydireccion); //direccion
|
||
|
|
rs485.print(",");
|
||
|
|
rs485.print((int)temp_angle); //valor del sensor
|
||
|
|
rs485.print("f"); //fin de trama
|
||
|
|
rs485.flush(); //Esperamos hasta que se envíen los datos
|
||
|
|
digitalWrite(EnTxPin, LOW); //RS485 como receptor
|
||
|
|
Serial.print("Slave ");
|
||
|
|
Serial.print(mydireccion);
|
||
|
|
Serial.print(" i am in ");
|
||
|
|
Serial.println(temp_angle);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (funcion == 'A')
|
||
|
|
{
|
||
|
|
if (rs485.read() == 'F') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
//int lectura = analogRead(0); //realizamos la lectura del sensor
|
||
|
|
float limit_angle = pulses_to_angle(pulses_max); //realizamos la lectura del senso
|
||
|
|
digitalWrite(EnTxPin, HIGH); //rs485 como transmisor
|
||
|
|
rs485.print("i"); //inicio de trama
|
||
|
|
rs485.print(mydireccion); //direccion
|
||
|
|
rs485.print(",");
|
||
|
|
rs485.print((int)limit_angle); //valor del sensor
|
||
|
|
rs485.print("f"); //fin de trama
|
||
|
|
rs485.flush(); //Esperamos hasta que se envíen los datos
|
||
|
|
digitalWrite(EnTxPin, LOW); //RS485 como receptor
|
||
|
|
Serial.print("Slave ");
|
||
|
|
Serial.print(mydireccion);
|
||
|
|
Serial.print(" my limit is ");
|
||
|
|
Serial.println(limit_angle);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
float temp_ang = actual_angle();
|
||
|
|
if (((temp_ang != angle_final)||((angle_final==0)&&(!stop_right))) && (!search_angle))
|
||
|
|
{
|
||
|
|
if ((angle_final==0)&&(!stop_right))
|
||
|
|
{
|
||
|
|
right_motor(100);
|
||
|
|
time_cero = millis();
|
||
|
|
while ((!stop_right)&&((millis()-time_cero)<=100000));// Serial.print(F("."));//delay(10);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
delay(100);
|
||
|
|
move_motor(angle_final, 100);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (flag_calibrate)
|
||
|
|
{
|
||
|
|
flag_left = false;
|
||
|
|
flag_right = false;
|
||
|
|
calibrate();
|
||
|
|
flag_calibrate=false;
|
||
|
|
}
|
||
|
|
|
||
|
|
if(flag_mov_motor)
|
||
|
|
{
|
||
|
|
move_motor(angle_mov, speed_max);
|
||
|
|
flag_mov_motor = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void timerRead(void)
|
||
|
|
{
|
||
|
|
if (flag_right)
|
||
|
|
{
|
||
|
|
if ((actual_angle() <= angle_final) && (search_angle))
|
||
|
|
{
|
||
|
|
md.disableDrivers();
|
||
|
|
md.setM1Speed(0);
|
||
|
|
//Serial.println("STOP");
|
||
|
|
flag_right = false;
|
||
|
|
search_angle = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
else if (flag_left)
|
||
|
|
{
|
||
|
|
//if (((actual_angle() >= angle_final)||((angle_final==0)&&stop_left))&& (search_angle))
|
||
|
|
if ((actual_angle() >= angle_final) && (search_angle))
|
||
|
|
{
|
||
|
|
md.disableDrivers();
|
||
|
|
md.setM1Speed(0);
|
||
|
|
flag_left = false;
|
||
|
|
search_angle = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
rs485read();
|
||
|
|
#if DEBUG
|
||
|
|
Serial.print("Pulsos: ");
|
||
|
|
Serial.print(motor_pulses());
|
||
|
|
Serial.print(" Angulo: ");
|
||
|
|
Serial.print(actual_angle());
|
||
|
|
Serial.print(" Angulo destino: ");
|
||
|
|
Serial.println(angle_final);
|
||
|
|
|
||
|
|
Serial.print(stop_right);
|
||
|
|
Serial.print(" ");
|
||
|
|
Serial.print(stop_left);
|
||
|
|
Serial.print(" ");
|
||
|
|
Serial.println(pulses_to_angle(pulses_max));
|
||
|
|
//
|
||
|
|
// Serial.print("M1 current: ");
|
||
|
|
// Serial.print(md.getM1CurrentMilliamps());
|
||
|
|
// Serial.println(" mA");
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
int motor_pulses()
|
||
|
|
{
|
||
|
|
return pulses;
|
||
|
|
}
|
||
|
|
|
||
|
|
void stopIfFault()
|
||
|
|
{
|
||
|
|
if (md.getM1Fault())
|
||
|
|
{
|
||
|
|
md.disableDrivers();
|
||
|
|
delay(1);
|
||
|
|
Serial.println("M1 fault");
|
||
|
|
while (1);
|
||
|
|
}
|
||
|
|
if (md.getM2Fault())
|
||
|
|
{
|
||
|
|
md.disableDrivers();
|
||
|
|
delay(1);
|
||
|
|
Serial.println("M2 fault");
|
||
|
|
while (1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
float angle_to_pulses(int val)
|
||
|
|
{
|
||
|
|
return ((float)val * 2400 * N) / 360;
|
||
|
|
}
|
||
|
|
|
||
|
|
float pulses_to_angle(int val)
|
||
|
|
{
|
||
|
|
return ((float)val * 360) / (2400 * N);
|
||
|
|
}
|
||
|
|
|
||
|
|
unsigned long time_calibrate = millis();
|
||
|
|
|
||
|
|
void calibrate()
|
||
|
|
{
|
||
|
|
Serial.print(F("Calibrating right"));
|
||
|
|
right_motor(100);
|
||
|
|
time_calibrate = millis();
|
||
|
|
while ((!stop_right)&&((millis()-time_calibrate)<=100000));// Serial.print(F("."));//delay(10);
|
||
|
|
Serial.println();
|
||
|
|
delay(2000);
|
||
|
|
Serial.print(F("Calibrating left"));
|
||
|
|
left_motor(100);
|
||
|
|
time_calibrate = millis();
|
||
|
|
while ((!stop_left)&&((millis()-time_calibrate)<=100000));// Serial.print(F("."));//delay(10);
|
||
|
|
Serial.println();
|
||
|
|
delay(2000);
|
||
|
|
Serial.println(F("Calibration finish"));
|
||
|
|
delay(1000);
|
||
|
|
// move_motor( pulses_to_angle(pulses_max)/2, 100);
|
||
|
|
}
|
||
|
|
|
||
|
|
volatile unsigned long time_ramp = 0;
|
||
|
|
|
||
|
|
//int right_motor(int speed)
|
||
|
|
//{
|
||
|
|
// if (!stop_right)
|
||
|
|
// {
|
||
|
|
// time_ramp = millis();
|
||
|
|
// flag_right = true;
|
||
|
|
// if (speed > MAXSPEED) speed = MAXSPEED;
|
||
|
|
// md.enableM1Driver();
|
||
|
|
// for (int i = 0; i >= -speed; i--) //RIGHT
|
||
|
|
// {
|
||
|
|
// while (((millis() - time_ramp) <= TIMERAMP) && (!stop_right));
|
||
|
|
// if (!stop_right)
|
||
|
|
// {
|
||
|
|
// speed_actual = i;
|
||
|
|
// md.setM1Speed(i);
|
||
|
|
// time_ramp = millis();
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
// return 0;
|
||
|
|
// }
|
||
|
|
// return 1;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
// #if DEBUG
|
||
|
|
// Serial.println("No se puede ir a la derecha");
|
||
|
|
// #endif
|
||
|
|
// return -1;
|
||
|
|
// }
|
||
|
|
//}
|
||
|
|
|
||
|
|
void right_motor(int speed)
|
||
|
|
{
|
||
|
|
if (!stop_right)
|
||
|
|
{
|
||
|
|
time_ramp = millis();
|
||
|
|
flag_right = true;
|
||
|
|
if (speed > MAXSPEED) speed = MAXSPEED;
|
||
|
|
md.enableM1Driver();
|
||
|
|
for (int i = 0; i >= -speed; i--) //RIGHT
|
||
|
|
{
|
||
|
|
while (((millis() - time_ramp) <= TIMERAMP) && (!stop_right));
|
||
|
|
if (!stop_right)
|
||
|
|
{
|
||
|
|
speed_actual = i;
|
||
|
|
md.setM1Speed(i);
|
||
|
|
time_ramp = millis();
|
||
|
|
}
|
||
|
|
else break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#if DEBUG
|
||
|
|
else Serial.println("No se puede ir a la derecha");
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//int left_motor(int speed)
|
||
|
|
//{
|
||
|
|
// if (!stop_left)
|
||
|
|
// {
|
||
|
|
// time_ramp = millis();
|
||
|
|
// flag_left = true;
|
||
|
|
// if (speed > MAXSPEED) speed = MAXSPEED;
|
||
|
|
// md.enableM1Driver();
|
||
|
|
// for (int i = 0; i <= speed; i++) //LEFT
|
||
|
|
// {
|
||
|
|
// speed_actual = i;
|
||
|
|
// while (((millis() - time_ramp) <= TIMERAMP) && (!stop_left));
|
||
|
|
// if (!stop_left)
|
||
|
|
// {
|
||
|
|
// md.setM1Speed(i);
|
||
|
|
// time_ramp = millis();
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
// return 0;
|
||
|
|
// }
|
||
|
|
// return 1;
|
||
|
|
// }
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
// #if DEBUG
|
||
|
|
// Serial.println("No se puede ir a la izquierda");
|
||
|
|
// #endif
|
||
|
|
// return -1;
|
||
|
|
// }
|
||
|
|
//}
|
||
|
|
|
||
|
|
void left_motor(int speed)
|
||
|
|
{
|
||
|
|
if (!stop_left)
|
||
|
|
{
|
||
|
|
time_ramp = millis();
|
||
|
|
flag_left = true;
|
||
|
|
if (speed > MAXSPEED) speed = MAXSPEED;
|
||
|
|
md.enableM1Driver();
|
||
|
|
for (int i = 0; i <= speed; i++) //LEFT
|
||
|
|
{
|
||
|
|
speed_actual = i;
|
||
|
|
while (((millis() - time_ramp) <= TIMERAMP) && (!stop_left));
|
||
|
|
if (!stop_left)
|
||
|
|
{
|
||
|
|
md.setM1Speed(i);
|
||
|
|
time_ramp = millis();
|
||
|
|
}
|
||
|
|
else break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#if DEBUG
|
||
|
|
else Serial.println("No se puede ir a la izquierda");
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
float actual_angle()
|
||
|
|
{
|
||
|
|
return pulses_to_angle(motor_pulses());
|
||
|
|
}
|
||
|
|
|
||
|
|
float temp_angle = actual_angle();
|
||
|
|
|
||
|
|
void move_motor(float angle, int speed)
|
||
|
|
{
|
||
|
|
temp_angle = actual_angle();
|
||
|
|
angle_final = angle;
|
||
|
|
if (temp_angle > angle)
|
||
|
|
{
|
||
|
|
search_angle = true;
|
||
|
|
right_motor(speed);
|
||
|
|
}
|
||
|
|
else if (temp_angle < angle)
|
||
|
|
{
|
||
|
|
search_angle = true;
|
||
|
|
left_motor(speed);
|
||
|
|
}
|
||
|
|
}
|