427 lines
11 KiB
Arduino
427 lines
11 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 = 101; //Direccion del esclavo
|
||
|
|
#define rs485 Serial2
|
||
|
|
|
||
|
|
#define fc1 18
|
||
|
|
#define fc2 19
|
||
|
|
#define DEBUG false
|
||
|
|
#define MAXSPEED 200
|
||
|
|
#define TIMERAMP 10
|
||
|
|
|
||
|
|
#define N 3.2 //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;
|
||
|
|
|
||
|
|
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
|
||
|
|
right_motor(100);
|
||
|
|
while (!stop_right);// Serial.print(F("."));//delay(10);
|
||
|
|
|
||
|
|
|
||
|
|
//calibrate();
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
|
||
|
|
int temp_ang = actual_angle();
|
||
|
|
if ((temp_ang != angle_final) && (!search_angle))
|
||
|
|
{
|
||
|
|
delay(100);
|
||
|
|
move_motor(angle_final, 100);
|
||
|
|
}
|
||
|
|
|
||
|
|
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')
|
||
|
|
{
|
||
|
|
int angulo = rs485.parseInt(); //recibimos el ángulo
|
||
|
|
if (rs485.read() == 'V') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
int speed = rs485.parseInt(); //recibimos la velocidad
|
||
|
|
|
||
|
|
if (rs485.read() == 'F') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
if (angulo < 512) //verificamos que sea un valor en el rango del servo
|
||
|
|
{
|
||
|
|
//myservo.write(angulo); //movemos el servomotor al ángulo correspondiente.
|
||
|
|
if (angulo==0)
|
||
|
|
{
|
||
|
|
right_motor(100);
|
||
|
|
while (!stop_right);// Serial.print(F("."));//delay(10);
|
||
|
|
}
|
||
|
|
else move_motor(angulo, speed);
|
||
|
|
Serial.print("Slave ");
|
||
|
|
Serial.print(mydireccion);
|
||
|
|
Serial.print(" go to ");
|
||
|
|
Serial.print(angulo);
|
||
|
|
Serial.print(" ");
|
||
|
|
Serial.println(speed);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (funcion == 'C')
|
||
|
|
{
|
||
|
|
if (rs485.read() == 'F') //Si el fin de trama es el correcto
|
||
|
|
{
|
||
|
|
calibrate();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
//---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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
delay(10);
|
||
|
|
}
|
||
|
|
|
||
|
|
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) && (search_angle))
|
||
|
|
{
|
||
|
|
md.disableDrivers();
|
||
|
|
md.setM1Speed(0);
|
||
|
|
flag_left = false;
|
||
|
|
search_angle = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//void enable_motor(){
|
||
|
|
// if ((!stop_left)&&(!stop_right)) md.enableM1Driver();
|
||
|
|
// else if ((stop_left)||(stop_right)) md.disableDrivers();
|
||
|
|
//}
|
||
|
|
//
|
||
|
|
//void disable_motor(){
|
||
|
|
// if ((stop_left)||(stop_right)) md.disableDrivers();
|
||
|
|
//}
|
||
|
|
|
||
|
|
float angle_to_pulses(int val)
|
||
|
|
{
|
||
|
|
return ((float)val * 2400 * N) / 360;
|
||
|
|
}
|
||
|
|
|
||
|
|
float pulses_to_angle(int val)
|
||
|
|
{
|
||
|
|
return ((float)val * 360) / (2400 * N);
|
||
|
|
}
|
||
|
|
|
||
|
|
void calibrate()
|
||
|
|
{
|
||
|
|
Serial.print(F("Calibrating right"));
|
||
|
|
right_motor(100);
|
||
|
|
while (!stop_right);// Serial.print(F("."));//delay(10);
|
||
|
|
Serial.println();
|
||
|
|
delay(2000);
|
||
|
|
Serial.print(F("Calibrating left"));
|
||
|
|
left_motor(100);
|
||
|
|
while (!stop_left);// Serial.print(F("."));//delay(10);
|
||
|
|
Serial.println();
|
||
|
|
delay(2000);
|
||
|
|
Serial.println(F("Calibration finish"));
|
||
|
|
delay(1000);
|
||
|
|
move_motor(0, 100);
|
||
|
|
}
|
||
|
|
|
||
|
|
volatile unsigned long time_ramp = 0;
|
||
|
|
|
||
|
|
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
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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(int 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);
|
||
|
|
}
|
||
|
|
}
|