[136] | 1 | //#include <ArduinoRobotMotorBoard.h>
|
---|
| 2 | #include "LineFollow.h"
|
---|
| 3 |
|
---|
| 4 | //#define KP 19 //0.1 units
|
---|
| 5 | //#define KD 14
|
---|
| 6 | //#define ROBOT_SPEED 100 //percentage
|
---|
| 7 |
|
---|
| 8 | //#define KP 11
|
---|
| 9 | //#define KD 5
|
---|
| 10 | //#define ROBOT_SPEED 50
|
---|
| 11 |
|
---|
| 12 | //#define INTEGRATION_TIME 10 //En ms
|
---|
| 13 |
|
---|
| 14 | /*uint8_t KP=11;
|
---|
| 15 | uint8_t KD=5;
|
---|
| 16 | uint8_t robotSpeed=50; //percentage
|
---|
| 17 | uint8_t intergrationTime=10;*/
|
---|
| 18 |
|
---|
| 19 | #define NIVEL_PARA_LINEA 50
|
---|
| 20 |
|
---|
| 21 | /*int lectura_sensor[5], last_error=0, acu=0;
|
---|
| 22 |
|
---|
| 23 | //Estos son los arrays que hay que rellenar con los valores de los sensores
|
---|
| 24 | //de suelo sobre blanco y negro.
|
---|
| 25 | int sensor_blanco[]={
|
---|
| 26 | 0,0,0,0,0};
|
---|
| 27 | int sensor_negro[]={
|
---|
| 28 | 1023,1023,1023,1023,1023};
|
---|
| 29 | */
|
---|
| 30 | //unsigned long time;
|
---|
| 31 |
|
---|
| 32 | //void mueve_robot(int vel_izq, int vel_der);
|
---|
| 33 | //void para_robot();
|
---|
| 34 | //void doCalibration(int speedPct, int time);
|
---|
| 35 | //void ajusta_niveles(); //calibrate values
|
---|
| 36 |
|
---|
| 37 | LineFollow::LineFollow(){
|
---|
| 38 | /*KP=11;
|
---|
| 39 | KD=5;
|
---|
| 40 | robotSpeed=50; //percentage
|
---|
| 41 | intergrationTime=10;*/
|
---|
| 42 | config(11,5,50,10);
|
---|
| 43 |
|
---|
| 44 | for(int i=0;i<5;i++){
|
---|
| 45 | sensor_blanco[i]=0;
|
---|
| 46 | sensor_negro[i]=1023;
|
---|
| 47 | }
|
---|
| 48 | }
|
---|
| 49 |
|
---|
| 50 | void LineFollow::config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){
|
---|
| 51 | this->KP=KP;
|
---|
| 52 | this->KD=KD;
|
---|
| 53 | this->robotSpeed=robotSpeed;
|
---|
| 54 | this->intergrationTime=intergrationTime;
|
---|
| 55 | /*Serial.print("LFC: ");
|
---|
| 56 | Serial.print(KP);
|
---|
| 57 | Serial.print(' ');
|
---|
| 58 | Serial.print(KD);
|
---|
| 59 | Serial.print(' ');
|
---|
| 60 | Serial.print(robotSpeed);
|
---|
| 61 | Serial.print(' ');
|
---|
| 62 | Serial.println(intergrationTime);*/
|
---|
| 63 |
|
---|
| 64 | }
|
---|
| 65 | void LineFollow::calibIRs(){
|
---|
| 66 | static bool isInited=false;//So only init once
|
---|
| 67 | if(isInited)return ;
|
---|
| 68 |
|
---|
| 69 | delay(1000);
|
---|
| 70 |
|
---|
| 71 | doCalibration(30,500);
|
---|
| 72 | doCalibration(-30,800);
|
---|
| 73 | doCalibration(30,500);
|
---|
| 74 |
|
---|
| 75 | delay(1000);
|
---|
| 76 | isInited=true;
|
---|
| 77 | }
|
---|
| 78 |
|
---|
| 79 | void LineFollow::runLineFollow(){
|
---|
| 80 | for(int count=0; count<5; count++)
|
---|
| 81 | {
|
---|
| 82 | lectura_sensor[count]=map(_IRread(count),sensor_negro[count],sensor_blanco[count],0,127);
|
---|
| 83 | acu+=lectura_sensor[count];
|
---|
| 84 | }
|
---|
| 85 |
|
---|
| 86 | //Serial.println(millis());
|
---|
| 87 | if (acu > NIVEL_PARA_LINEA)
|
---|
| 88 | {
|
---|
| 89 | acu/=5;
|
---|
| 90 |
|
---|
| 91 | int error = ((lectura_sensor[0]<<6)+(lectura_sensor[1]<<5)-(lectura_sensor[3]<<5)-(lectura_sensor[4]<<6))/acu;
|
---|
| 92 |
|
---|
| 93 | error = constrain(error,-100,100);
|
---|
| 94 |
|
---|
| 95 | //Calculamos la correcion de velocidad mediante un filtro PD
|
---|
| 96 | int vel = (error * KP)/10 + (error-last_error)*KD;
|
---|
| 97 |
|
---|
| 98 | last_error = error;
|
---|
| 99 |
|
---|
| 100 | //Corregimos la velocidad de avance con el error de salida del filtro PD
|
---|
| 101 | int motor_left = constrain((robotSpeed + vel),-100,100);
|
---|
| 102 | int motor_right =constrain((robotSpeed - vel),-100,100);
|
---|
| 103 |
|
---|
| 104 | //Movemos el robot
|
---|
| 105 | //motorsWritePct(motor_left,motor_right);
|
---|
| 106 | motorsWritePct(motor_left,motor_right);
|
---|
| 107 |
|
---|
| 108 | //Esperamos un poquito a que el robot reaccione
|
---|
| 109 | delay(intergrationTime);
|
---|
| 110 | }
|
---|
| 111 | else
|
---|
| 112 | {
|
---|
| 113 | //Hemos encontrado una linea negra
|
---|
| 114 | //perpendicular a nuestro camino
|
---|
| 115 | //paramos el robot
|
---|
| 116 | motorsStop();
|
---|
| 117 |
|
---|
| 118 | //y detenemos la ejecución del programa
|
---|
| 119 | //while(true);
|
---|
| 120 | reportActionDone();
|
---|
| 121 | //setMode(MODE_SIMPLE);
|
---|
| 122 | }
|
---|
| 123 | }
|
---|
| 124 |
|
---|
| 125 |
|
---|
| 126 | void LineFollow::doCalibration(int speedPct, int time){
|
---|
| 127 | motorsWritePct(speedPct, -speedPct);
|
---|
| 128 | unsigned long beginTime = millis();
|
---|
| 129 | while((millis()-beginTime)<time)
|
---|
| 130 | ajusta_niveles();
|
---|
| 131 | motorsStop();
|
---|
| 132 | }
|
---|
| 133 | void LineFollow::ajusta_niveles()
|
---|
| 134 | {
|
---|
| 135 | int lectura=0;
|
---|
| 136 |
|
---|
| 137 | for(int count=0; count<5; count++){
|
---|
| 138 | lectura=_IRread(count);
|
---|
| 139 |
|
---|
| 140 | if (lectura > sensor_blanco[count])
|
---|
| 141 | sensor_blanco[count]=lectura;
|
---|
| 142 |
|
---|
| 143 | if (lectura < sensor_negro[count])
|
---|
| 144 | sensor_negro[count]=lectura;
|
---|
| 145 | }
|
---|
| 146 | }
|
---|
| 147 |
|
---|
| 148 |
|
---|
| 149 |
|
---|
| 150 |
|
---|
| 151 |
|
---|
| 152 |
|
---|