367 lines
8.2 KiB
C++
367 lines
8.2 KiB
C++
//int ir[8] = { A2, A3, A4, A5, A6, A7, A8, A9 };
|
|
int ir[8] = { A9, A8, A7, A6, A5, A4, A3, A2 };
|
|
int fc = A10;
|
|
int INA[5] = { 5, 6, 14, 18, 20};
|
|
int INB[5] = { 4, 3, 2, 17, 21};
|
|
int EN[5] = { 8, 7, 15, 16, 19};
|
|
int CS[5] = {A11, A12, A13, A14, A15};
|
|
int pwm[5] = { 13, 12, 11, 10, 9};
|
|
int ir_value[8] = {0, 0, 0, 0, 0, 0, 0, 0};
|
|
int ir_value_ant[8] = {0, 0, 0, 0, 0, 0, 0, 0};
|
|
|
|
#define BRAKE 0
|
|
#define CW 1
|
|
#define CCW 2
|
|
#define CS_THRESHOLD 15 // Definition of safety current (Check: "1.3 Monster Shield Example").
|
|
#define N 1
|
|
|
|
|
|
|
|
#define debug false
|
|
|
|
short usSpeed = 150; //default motor speed
|
|
unsigned short usMotor_Status = BRAKE;
|
|
|
|
bool avanza_state = false;
|
|
bool retrocede_state = false;
|
|
|
|
void setup() {
|
|
pinMode(fc, INPUT);
|
|
for(int i=0; i<5; i++)
|
|
{
|
|
pinMode(INA[i], OUTPUT);
|
|
pinMode(INB[i], OUTPUT);
|
|
pinMode(EN[i], OUTPUT);
|
|
digitalWrite(EN[i], HIGH);
|
|
//pinMode(CS[i], INPUT);
|
|
}
|
|
|
|
Serial.begin(115200); // Initiates the serial to do the monitoring
|
|
#if debug
|
|
Serial.println("Begin motor control");
|
|
Serial.println(); //Print function list for user selection
|
|
Serial.println("Enter number for control option:");
|
|
Serial.println("1. STOP");
|
|
Serial.println("2. FORWARD");
|
|
Serial.println("3. REVERSE");
|
|
Serial.println("4. READ CURRENT");
|
|
Serial.println("+. INCREASE SPEED");
|
|
Serial.println("-. DECREASE SPEED");
|
|
Serial.println("q. FRONT");
|
|
Serial.println("w. AVANZA");
|
|
Serial.println("s. RETROCEDE");
|
|
Serial.println("a. IZQUIERDA");
|
|
Serial.println("d. DERECHA");
|
|
Serial.println();
|
|
#endif
|
|
}
|
|
|
|
//ir 0 frontal
|
|
//ir 1 frontal izquierdo
|
|
//ir 2 izquierdo
|
|
//ir 3 trasero izquierdo
|
|
//ir 4 trasero
|
|
//ir 5 trasero derecho
|
|
//ir 6 derecho
|
|
//ir 7 frontal derecho
|
|
#define LIMIT 300
|
|
#define LIMIT_THRESHOLD 10
|
|
|
|
void loop() {
|
|
|
|
|
|
//if(!digitalRead(fc)) motorGo(4, BRAKE, 0);
|
|
manual_control();
|
|
stop_move();
|
|
ir_read();
|
|
}
|
|
|
|
void stop_move()
|
|
{
|
|
if ((ir_value[0]<=LIMIT)&&(avanza_state))
|
|
{
|
|
motorGo(0, BRAKE, 0);
|
|
motorGo(1, BRAKE, 0);
|
|
avanza_state = false;
|
|
#if debug
|
|
Serial.println("Stop Avanza");
|
|
#endif
|
|
}
|
|
if ((ir_value[4]<=LIMIT)&&(retrocede_state))
|
|
{
|
|
motorGo(0, BRAKE, 0);
|
|
motorGo(1, BRAKE, 0);
|
|
retrocede_state = false;
|
|
#if debug
|
|
Serial.println("Stop Retrocede");
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void manual_control()
|
|
{
|
|
char user_input;
|
|
while(Serial.available())
|
|
{
|
|
user_input = Serial.read(); //Read user input and trigger appropriate function
|
|
if (user_input =='p')
|
|
{
|
|
Stop();
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='j')
|
|
{
|
|
Forward();
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='k')
|
|
{
|
|
Reverse();
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='l')
|
|
{
|
|
for(int i=0; i<5; i++)
|
|
{
|
|
float voltage = 0;
|
|
for(int j=0; j<N; j++)
|
|
{
|
|
voltage = analogRead(CS[i])*(5000/1023.) + voltage;
|
|
}
|
|
#if debug
|
|
Serial.print(voltage/(N*0.13));
|
|
Serial.print(' ');
|
|
#endif
|
|
}
|
|
Serial.write(user_input);
|
|
#if debug
|
|
Serial.println();
|
|
#endif
|
|
}
|
|
else if(user_input =='+')
|
|
{
|
|
IncreaseSpeed();
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='-')
|
|
{
|
|
DecreaseSpeed();
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='q')
|
|
{
|
|
front(100);
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='w')
|
|
{
|
|
avanza(100);
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='s')
|
|
{
|
|
retrocede(100);
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='a')
|
|
{
|
|
izquierda(100);
|
|
Serial.write(user_input);
|
|
}
|
|
else if(user_input =='d')
|
|
{
|
|
derecha(100);
|
|
Serial.write(user_input);
|
|
}
|
|
else
|
|
{
|
|
Serial.print('?');
|
|
}
|
|
}
|
|
}
|
|
//convert voltage to millimeters
|
|
int convertIRvoltsToMM(float v) {
|
|
float value = -0.00003983993846*v*v*v+ 0.0456899769 *v*v - 17.48535575 * v + 2571.052715;
|
|
if (value < 200) value = 200;
|
|
else if (value > 1500) value = 1500;
|
|
return value;
|
|
}
|
|
|
|
float average(int pin)
|
|
{
|
|
float val = 0;
|
|
for(int i=0; i<N; i++)
|
|
{
|
|
val = val + analogRead(ir[pin]);
|
|
delay(1);
|
|
}
|
|
|
|
return convertIRvoltsToMM(val/N);
|
|
}
|
|
|
|
#define RES 50
|
|
void ir_read()
|
|
{
|
|
for(int i=0; i<8; i++)
|
|
{
|
|
ir_value[i] = average(i);
|
|
if ((ir_value[i]<=(ir_value_ant[i]-RES))||(ir_value[i]>=(ir_value_ant[i]+RES)))
|
|
{
|
|
ir_value_ant[i]= ir_value[i];
|
|
Serial.print("IR");
|
|
Serial.print(i);
|
|
Serial.print(":");
|
|
Serial.println(ir_value[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void Stop()
|
|
{
|
|
#if debug
|
|
Serial.println("Stop");
|
|
#endif
|
|
usMotor_Status = BRAKE;
|
|
for(int i=0; i<5; i++) motorGo(i, usMotor_Status, 0);
|
|
}
|
|
|
|
void Forward()
|
|
{
|
|
#if debug
|
|
Serial.println("Forward");
|
|
#endif
|
|
usMotor_Status = CW;
|
|
for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
|
|
}
|
|
|
|
void Reverse()
|
|
{
|
|
#if debug
|
|
Serial.println("Reverse");
|
|
#endif
|
|
usMotor_Status = CCW;
|
|
for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
|
|
}
|
|
|
|
void IncreaseSpeed()
|
|
{
|
|
usSpeed = usSpeed + 10;
|
|
if(usSpeed > 255)
|
|
{
|
|
usSpeed = 255;
|
|
}
|
|
#if debug
|
|
Serial.print("Speed +: ");
|
|
Serial.println(usSpeed);
|
|
#endif
|
|
for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
|
|
}
|
|
|
|
void DecreaseSpeed()
|
|
{
|
|
usSpeed = usSpeed - 10;
|
|
if(usSpeed < 0)
|
|
{
|
|
usSpeed = 0;
|
|
}
|
|
#if debug
|
|
Serial.print("Speed -: ");
|
|
Serial.println(usSpeed);
|
|
#endif
|
|
for(int i=2; i<5; i++) motorGo(i, usMotor_Status, usSpeed);
|
|
}
|
|
|
|
void motorGo(uint8_t motor, uint8_t direct, uint8_t value) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e value (entra 0 e 255);
|
|
{
|
|
if(direct == CW)
|
|
{
|
|
digitalWrite(INA[motor], LOW);
|
|
digitalWrite(INB[motor], HIGH);
|
|
}
|
|
else if(direct == CCW)
|
|
{
|
|
digitalWrite(INA[motor], HIGH);
|
|
digitalWrite(INB[motor], LOW);
|
|
}
|
|
else
|
|
{
|
|
digitalWrite(INA[motor], LOW);
|
|
digitalWrite(INB[motor], LOW);
|
|
}
|
|
analogWrite(pwm[motor], value);
|
|
}
|
|
|
|
void front(uint8_t value) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e value (entra 0 e 255);
|
|
{
|
|
#if debug
|
|
Serial.println("Front");
|
|
#endif
|
|
while(digitalRead(fc)) motorGo(4, CW, value);
|
|
motorGo(4, BRAKE, 0);
|
|
}
|
|
|
|
void avanza(uint8_t value) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e value (entra 0 e 255);
|
|
{
|
|
if (ir_value[0]>LIMIT)
|
|
{
|
|
#if debug
|
|
Serial.println("Avanza");
|
|
#endif
|
|
motorGo(0, CCW, value);
|
|
motorGo(1, CCW, value);
|
|
if (value>0) avanza_state = true;
|
|
else avanza_state = false;
|
|
}
|
|
else
|
|
{
|
|
#if debug
|
|
Serial.println("Stop Avanza");
|
|
#endif
|
|
motorGo(0, BRAKE, 0);
|
|
motorGo(1, BRAKE, 0);
|
|
avanza_state = false;
|
|
}
|
|
}
|
|
|
|
void retrocede(uint8_t value) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e value (entra 0 e 255);
|
|
{
|
|
if (ir_value[4]>LIMIT)
|
|
{
|
|
#if debug
|
|
Serial.println("Retrocede");
|
|
#endif
|
|
motorGo(0, CW, value);
|
|
motorGo(1, CW, value);
|
|
if (value>0) retrocede_state = true;
|
|
else retrocede_state = false;
|
|
}
|
|
else
|
|
{
|
|
#if debug
|
|
Serial.println("Stop Retrocede");
|
|
#endif
|
|
motorGo(0, BRAKE, 0);
|
|
motorGo(1, BRAKE, 0);
|
|
retrocede_state = false;
|
|
}
|
|
}
|
|
|
|
void izquierda(uint8_t value) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e value (entra 0 e 255);
|
|
{
|
|
#if debug
|
|
Serial.println("Izquierda");
|
|
#endif
|
|
motorGo(0, CW, value);
|
|
motorGo(1, CCW, value);
|
|
}
|
|
|
|
void derecha(uint8_t value) //Function that controls the variables: motor(0 ou 1), direction (cw ou ccw) e value (entra 0 e 255);
|
|
{
|
|
#if debug
|
|
Serial.println("Derecha");
|
|
#endif
|
|
motorGo(0, CCW, value);
|
|
motorGo(1, CW, value);
|
|
}
|
|
|