mirror of
https://github.com/arduino/Arduino.git
synced 2024-11-30 11:24:12 +01:00
153 lines
3.2 KiB
C++
153 lines
3.2 KiB
C++
//#include <ArduinoRobotMotorBoard.h>
|
||
#include "LineFollow.h"
|
||
|
||
//#define KP 19 //0.1 units
|
||
//#define KD 14
|
||
//#define ROBOT_SPEED 100 //percentage
|
||
|
||
//#define KP 11
|
||
//#define KD 5
|
||
//#define ROBOT_SPEED 50
|
||
|
||
//#define INTEGRATION_TIME 10 //En ms
|
||
|
||
/*uint8_t KP=11;
|
||
uint8_t KD=5;
|
||
uint8_t robotSpeed=50; //percentage
|
||
uint8_t intergrationTime=10;*/
|
||
|
||
#define NIVEL_PARA_LINEA 50
|
||
|
||
/*int lectura_sensor[5], last_error=0, acu=0;
|
||
|
||
//Estos son los arrays que hay que rellenar con los valores de los sensores
|
||
//de suelo sobre blanco y negro.
|
||
int sensor_blanco[]={
|
||
0,0,0,0,0};
|
||
int sensor_negro[]={
|
||
1023,1023,1023,1023,1023};
|
||
*/
|
||
//unsigned long time;
|
||
|
||
//void mueve_robot(int vel_izq, int vel_der);
|
||
//void para_robot();
|
||
//void doCalibration(int speedPct, int time);
|
||
//void ajusta_niveles(); //calibrate values
|
||
|
||
LineFollow::LineFollow(){
|
||
/*KP=11;
|
||
KD=5;
|
||
robotSpeed=50; //percentage
|
||
intergrationTime=10;*/
|
||
config(11,5,50,10);
|
||
|
||
for(int i=0;i<5;i++){
|
||
sensor_blanco[i]=0;
|
||
sensor_negro[i]=1023;
|
||
}
|
||
}
|
||
|
||
void LineFollow::config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){
|
||
this->KP=KP;
|
||
this->KD=KD;
|
||
this->robotSpeed=robotSpeed;
|
||
this->intergrationTime=intergrationTime;
|
||
/*Serial.print("LFC: ");
|
||
Serial.print(KP);
|
||
Serial.print(' ');
|
||
Serial.print(KD);
|
||
Serial.print(' ');
|
||
Serial.print(robotSpeed);
|
||
Serial.print(' ');
|
||
Serial.println(intergrationTime);*/
|
||
|
||
}
|
||
void LineFollow::calibIRs(){
|
||
static bool isInited=false;//So only init once
|
||
if(isInited)return ;
|
||
|
||
delay(1000);
|
||
|
||
doCalibration(30,500);
|
||
doCalibration(-30,800);
|
||
doCalibration(30,500);
|
||
|
||
delay(1000);
|
||
isInited=true;
|
||
}
|
||
|
||
void LineFollow::runLineFollow(){
|
||
for(int count=0; count<5; count++)
|
||
{
|
||
lectura_sensor[count]=map(IRread(count),sensor_negro[count],sensor_blanco[count],0,127);
|
||
acu+=lectura_sensor[count];
|
||
}
|
||
|
||
//Serial.println(millis());
|
||
if (acu > NIVEL_PARA_LINEA)
|
||
{
|
||
acu/=5;
|
||
|
||
int error = ((lectura_sensor[0]<<6)+(lectura_sensor[1]<<5)-(lectura_sensor[3]<<5)-(lectura_sensor[4]<<6))/acu;
|
||
|
||
error = constrain(error,-100,100);
|
||
|
||
//Calculamos la correcion de velocidad mediante un filtro PD
|
||
int vel = (error * KP)/10 + (error-last_error)*KD;
|
||
|
||
last_error = error;
|
||
|
||
//Corregimos la velocidad de avance con el error de salida del filtro PD
|
||
int motor_left = constrain((robotSpeed + vel),-100,100);
|
||
int motor_right =constrain((robotSpeed - vel),-100,100);
|
||
|
||
//Movemos el robot
|
||
//motorsWritePct(motor_left,motor_right);
|
||
motorsWritePct(motor_left,motor_right);
|
||
|
||
//Esperamos un poquito a que el robot reaccione
|
||
delay(intergrationTime);
|
||
}
|
||
else
|
||
{
|
||
//Hemos encontrado una linea negra
|
||
//perpendicular a nuestro camino
|
||
//paramos el robot
|
||
motorsStop();
|
||
|
||
//y detenemos la ejecuci<63>n del programa
|
||
//while(true);
|
||
reportActionDone();
|
||
//setMode(MODE_SIMPLE);
|
||
}
|
||
}
|
||
|
||
|
||
void LineFollow::doCalibration(int speedPct, int time){
|
||
motorsWritePct(speedPct, -speedPct);
|
||
unsigned long beginTime = millis();
|
||
while((millis()-beginTime)<time)
|
||
ajusta_niveles();
|
||
motorsStop();
|
||
}
|
||
void LineFollow::ajusta_niveles()
|
||
{
|
||
int lectura=0;
|
||
|
||
for(int count=0; count<5; count++){
|
||
lectura=IRread(count);
|
||
|
||
if (lectura > sensor_blanco[count])
|
||
sensor_blanco[count]=lectura;
|
||
|
||
if (lectura < sensor_negro[count])
|
||
sensor_negro[count]=lectura;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|