Hola,
Les comparto el robot esquivador de obstáculos llamado 'SandWorot V3', el material a usar fue:
La duración del proyecto SW v3 fue de:
Algunas Fotos del Robot:
Les comparto el robot esquivador de obstáculos llamado 'SandWorot V3', el material a usar fue:
La duración del proyecto SW v3 fue de:
Algunas Fotos del Robot:
Código:
/***************************************
Incluyo Librerias
****************************************/
#include <Ultrasonic.h>
#include <Servo.h>
/***************************************
Incluyo Clases y/o Definiciones
****************************************/
Ultrasonic hc_04(9,8);
Servo hd_1440A;
#define MIDDLE 90
#define POSITION_MAX 5
#define SpeedControlPin 3
#define Forward 1
#define Reverse 2
#define TurnLeft 3
#define TurnRight 4
#define TurnLeftGyro 5
#define TurnRightGyro 6
#define StopMotors 7
/***************************************
Incluyo Variables Globales
****************************************/
int objects[5]={0,0,0,0,0};
int pos=0;
int pot=0;
int lpwm=0;
int SpeedVar=0;
int MotorAFoward=6;
int MotorAReverse=5;
int MotorBFoward=4;
int MotorBReverse=2;
int direct=5;
/*******************************************************************************
* @brief: Inicializo perifericos
* @param[in] None
* @return None
*******************************************************************************/
void setup(){
hd_1440A.attach(7);
pinMode(SpeedControlPin,OUTPUT);
pinMode(MotorAFoward,OUTPUT);
pinMode(MotorAReverse,OUTPUT);
pinMode(MotorBFoward,OUTPUT);
pinMode(MotorBReverse,OUTPUT);
Serial.begin(9600);
analogWrite(3,50);
delay(2000);
}
/*******************************************************************************
* @brief: Ciclo permanente
* @param[in] None
* @return None
*******************************************************************************/
void loop(){
Get_Objects(objects);
direct=Set_Direction();
switch(direct){
case 0:
Set_Motor_Direction(TurnRightGyro,150);
delay(1500);
break;
case 1:
Set_Motor_Direction(TurnRightGyro,150);
delay(750);
break;
case 2:
Set_Motor_Direction(Forward,150);
while(hc_04.Ranging(CM)>20){
delay(100);
}
Set_Motor_Direction(Reverse,150);
delay(300);
break;
case 3:
Set_Motor_Direction(TurnLeftGyro,150);
delay(750);
break;
case 4:
Set_Motor_Direction(TurnLeftGyro,150);
delay(1500);
break;
case 5:
Set_Motor_Direction(TurnLeftGyro,150);
delay(3000);
break;
default:
Set_Motor_Direction(StopMotors,0);
break;
}
if(direct!=2){
Set_Motor_Direction(Forward,150);
while(hc_04.Ranging(CM)>20){
delay(100);
}
Set_Motor_Direction(Reverse,150);
delay(300);
}
Set_Motor_Direction(StopMotors,0);
}
/*******************************************************************************
* @brief: Establece la direccion del cual el robot tomara
* @param[in] None
* @return value: Valor que corresponde a la direccion que va dar los motores
*******************************************************************************/
int Set_Direction(){
int value=0,i=0;
int objectAux=0;
if(objects[0]>objects[1]){
value=0;
objectAux=objects[0];
}else{
value=1;
objectAux=objects[1];
}
if(objects[2]>objectAux){
value=2;
objectAux=objects[2];
}else{
value=value;
}
if(objects[3]>objectAux){
value=3;
objectAux=objects[3];
}else{
value=value;
}
if(objects[4]>objectAux){
value=4;
objectAux=objects[4];
}else{
value=value;
}
if(objects[0]>50 && objects[1]>50 && objects[2]>50 && objects[3]>50 && objects[4]>50){
value=2;
}else if (objects[0]<10 && objects[1]<10 && objects[2]<10 && objects[3]<10 && objects[4]<10){
value=5;
}
return value;
}
/*******************************************************************************
* @brief: Guarda todas las distancias de 5 puntos, cada punto es de 45 grados
* @param[in] *ptr: Permite alojar un arreglo de datos
* @return None
*******************************************************************************/
void Get_Objects(int *ptr){
for(pos=0;pos<POSITION_MAX;pos++){
hd_1440A.write(pos*45);
delay(220);
*ptr=hc_04.Ranging(CM);
ptr++;
}
hd_1440A.write(MIDDLE);
delay(200);
}
/*******************************************************************************
* @brief: Activa motores
* @param[in] *MA: Selecciona Motor A
* @param[in] *StateMA: Decide si prende o apaga MotorA
* @param[in] *MB: Selecciona Motor B
* @param[in] *StateMB: Decide si prende o apaga MotorB
* @return None
*******************************************************************************/
void Motors(int MA,int StateMA,int MB,int StateMB){
if(StateMA==1)
digitalWrite(MA,HIGH);
else
digitalWrite(MA,LOW);
if(StateMB==1)
digitalWrite(MB,HIGH);
else
digitalWrite(MB,LOW);
}
/*******************************************************************************
* @brief: Establece la direccion del Motor
* @param[in] *Dir: Direccion hacia donde debe de girar el robot
* @param[in] *Speed: Velocidad con la cual ejecutara la direccion
* @return None
*******************************************************************************/
void Set_Motor_Direction(int Dir,int Speed){
analogWrite(SpeedControlPin,Speed);
switch(Dir){
case Forward:
Motors(MotorAReverse,0,MotorBReverse,0);
Motors(MotorAFoward,1,MotorBFoward,1);
break;
case Reverse:
Motors(MotorAFoward,0,MotorBFoward,0);
Motors(MotorAReverse,1,MotorBReverse,1);
break;
case TurnLeft:
Motors(MotorAReverse,0,MotorBReverse,0);
Motors(MotorAFoward,0,MotorBFoward,1);
break;
case TurnRight:
Motors(MotorAReverse,0,MotorBReverse,0);
Motors(MotorAFoward,1,MotorBFoward,0);
break;
case TurnLeftGyro:
Motors(MotorAReverse,1,MotorBReverse,0);
Motors(MotorAFoward,0,MotorBFoward,1);
break;
case TurnRightGyro:
Motors(MotorAReverse,0,MotorBReverse,1);
Motors(MotorAFoward,1,MotorBFoward,0);
break;
default:
Motors(MotorAReverse,0,MotorBReverse,0);
Motors(MotorAFoward,0,MotorBFoward,0);
break;
}
}
