aboutsummaryrefslogtreecommitdiff
path: root/libraries/Robot_Motor/lineFollow.cpp
diff options
context:
space:
mode:
authorCristian Maglie <c.maglie@bug.st>2013-08-23 15:59:24 +0200
committerCristian Maglie <c.maglie@bug.st>2013-08-23 15:59:24 +0200
commit540743129b2badb813b703208d121ff14553c147 (patch)
tree6fadb4ebce68e1f0cb298a282be135c23fd156ed /libraries/Robot_Motor/lineFollow.cpp
parent073b3ac9d4ae93ac0bb3a91afc65ae9d8f1d5d59 (diff)
parent67c84855c2f3ce99b091a756bb2ca1a016260659 (diff)
Merge branch 'ide-1.5.x' into dev-ide-1.5.x-discovery
Conflicts: app/src/processing/app/Preferences.java app/src/processing/app/debug/Uploader.java
Diffstat (limited to 'libraries/Robot_Motor/lineFollow.cpp')
-rw-r--r--libraries/Robot_Motor/lineFollow.cpp152
1 files changed, 0 insertions, 152 deletions
diff --git a/libraries/Robot_Motor/lineFollow.cpp b/libraries/Robot_Motor/lineFollow.cpp
deleted file mode 100644
index d6ebed8..0000000
--- a/libraries/Robot_Motor/lineFollow.cpp
+++ /dev/null
@@ -1,152 +0,0 @@
-//#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ó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;
- }
-}
-
-
-
-
-
-