arduino/esp8266
1
0
Fork 0
mirror of https://github.com/esp8266/Arduino.git synced 2025-07-30 16:24:09 +03:00
Code Activity

SPI library to the new format and moved Robot_Motor and Robot_Control libraries

Browse Source
This commit is contained in:
Fede85
2013-07-03 22:00:02 +02:00
parent 87d3b4f56b
commit 2371e2ce0d
98 changed files with 14 additions and 216 deletions
Download Patch File Download Diff File Expand all files Collapse all files

265
libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp Normal file
View File

@ -0,0 +1,265 @@
#include "ArduinoRobotMotorBoard.h"
#include "EasyTransfer2.h"
#include "Multiplexer.h"
#include "LineFollow.h"
RobotMotorBoard::RobotMotorBoard(){
//LineFollow::LineFollow();
}
/*void RobotMotorBoard::beginIRReceiver(){
IRrecv::enableIRIn();
}*/
void RobotMotorBoard::begin(){
//initialze communication
Serial1.begin(9600);
messageIn.begin(&Serial1);
messageOut.begin(&Serial1);
//init MUX
uint8_t MuxPins[]={MUXA,MUXB,MUXC};
this->IRs.begin(MuxPins,MUX_IN,3);
pinMode(MUXI,INPUT);
digitalWrite(MUXI,LOW);
isPaused=false;
}
void RobotMotorBoard::process(){
if(isPaused)return;//skip process if the mode is paused
if(mode==MODE_SIMPLE){
//Serial.println("s");
//do nothing? Simple mode is just about getting commands
}else if(mode==MODE_LINE_FOLLOW){
//do line following stuff here.
LineFollow::runLineFollow();
}else if(mode==MODE_ADJUST_MOTOR){
//Serial.println('a');
//motorAdjustment=analogRead(POT);
//setSpeed(255,255);
//delay(100);
}
}
void RobotMotorBoard::pauseMode(bool onOff){
if(onOff){
isPaused=true;
}else{
isPaused=false;
}
stopCurrentActions();
}
void RobotMotorBoard::parseCommand(){
uint8_t modeName;
uint8_t codename;
int value;
int speedL;
int speedR;
if(this->messageIn.receiveData()){
//Serial.println("data received");
uint8_t command=messageIn.readByte();
//Serial.println(command);
switch(command){
case COMMAND_SWITCH_MODE:
modeName=messageIn.readByte();
setMode(modeName);
break;
case COMMAND_RUN:
if(mode==MODE_LINE_FOLLOW)break;//in follow line mode, the motor does not follow commands
speedL=messageIn.readInt();
speedR=messageIn.readInt();
motorsWrite(speedL,speedR);
break;
case COMMAND_MOTORS_STOP:
motorsStop();
break;
case COMMAND_ANALOG_WRITE:
codename=messageIn.readByte();
value=messageIn.readInt();
_analogWrite(codename,value);
break;
case COMMAND_DIGITAL_WRITE:
codename=messageIn.readByte();
value=messageIn.readByte();
_digitalWrite(codename,value);
break;
case COMMAND_ANALOG_READ:
codename=messageIn.readByte();
_analogRead(codename);
break;
case COMMAND_DIGITAL_READ:
codename=messageIn.readByte();
_digitalRead(codename);
break;
case COMMAND_READ_IR:
_readIR();
break;
case COMMAND_READ_TRIM:
_readTrim();
break;
case COMMAND_PAUSE_MODE:
pauseMode(messageIn.readByte());//onOff state
break;
case COMMAND_LINE_FOLLOW_CONFIG:
LineFollow::config(
messageIn.readByte(), //KP
messageIn.readByte(), //KD
messageIn.readByte(), //robotSpeed
messageIn.readByte() //IntegrationTime
);
break;
}
}
//delay(5);
}
uint8_t RobotMotorBoard::parseCodename(uint8_t codename){
switch(codename){
case B_TK1:
return TK1;
case B_TK2:
return TK2;
case B_TK3:
return TK3;
case B_TK4:
return TK4;
}
}
uint8_t RobotMotorBoard::codenameToAPin(uint8_t codename){
switch(codename){
case B_TK1:
return A0;
case B_TK2:
return A1;
case B_TK3:
return A6;
case B_TK4:
return A11;
}
}
void RobotMotorBoard::setMode(uint8_t mode){
if(mode==MODE_LINE_FOLLOW){
LineFollow::calibIRs();
}
/*if(mode==SET_MOTOR_ADJUSTMENT){
save_motor_adjustment_to_EEPROM();
}
*/
/*if(mode==MODE_IR_CONTROL){
beginIRReceiver();
}*/
this->mode=mode;
//stopCurrentActions();//If line following, this should stop the motors
}
void RobotMotorBoard::stopCurrentActions(){
motorsStop();
//motorsWrite(0,0);
}
void RobotMotorBoard::motorsWrite(int speedL, int speedR){
/*Serial.print(speedL);
Serial.print(" ");
Serial.println(speedR);*/
//motor adjustment, using percentage
_refreshMotorAdjustment();
if(motorAdjustment<0){
speedR*=(1+motorAdjustment);
}else{
speedL*=(1-motorAdjustment);
}
if(speedL>0){
analogWrite(IN_A1,speedL);
analogWrite(IN_A2,0);
}else{
analogWrite(IN_A1,0);
analogWrite(IN_A2,-speedL);
}
if(speedR>0){
analogWrite(IN_B1,speedR);
analogWrite(IN_B2,0);
}else{
analogWrite(IN_B1,0);
analogWrite(IN_B2,-speedR);
}
}
void RobotMotorBoard::motorsWritePct(int speedLpct, int speedRpct){
//speedLpct, speedRpct ranges from -100 to 100
motorsWrite(speedLpct*2.55,speedRpct*2.55);
}
void RobotMotorBoard::motorsStop(){
analogWrite(IN_A1,255);
analogWrite(IN_A2,255);
analogWrite(IN_B1,255);
analogWrite(IN_B2,255);
}
/*
*
*
* Input and Output ports
*
*
*/
void RobotMotorBoard::_digitalWrite(uint8_t codename,bool value){
uint8_t pin=parseCodename(codename);
digitalWrite(pin,value);
}
void RobotMotorBoard::_analogWrite(uint8_t codename,int value){
//There's no PWM available on motor board
}
void RobotMotorBoard::_digitalRead(uint8_t codename){
uint8_t pin=parseCodename(codename);
bool value=digitalRead(pin);
messageOut.writeByte(COMMAND_DIGITAL_READ_RE);
messageOut.writeByte(codename);
messageOut.writeByte(value);
messageOut.sendData();
}
void RobotMotorBoard::_analogRead(uint8_t codename){
uint8_t pin=codenameToAPin(codename);
int value=analogRead(pin);
messageOut.writeByte(COMMAND_ANALOG_READ_RE);
messageOut.writeByte(codename);
messageOut.writeInt(value);
messageOut.sendData();
}
int RobotMotorBoard::IRread(uint8_t num){
IRs.selectPin(num-1); //To make consistant with the pins labeled on the board
return IRs.getAnalogValue();
}
void RobotMotorBoard::_readIR(){
//Serial.println("readIR");
int value;
messageOut.writeByte(COMMAND_READ_IR_RE);
for(int i=1;i<6;i++){
value=IRread(i);
messageOut.writeInt(value);
}
messageOut.sendData();
}
void RobotMotorBoard::_readTrim(){
int value=analogRead(TRIM);
messageOut.writeByte(COMMAND_READ_TRIM_RE);
messageOut.writeInt(value);
messageOut.sendData();
}
void RobotMotorBoard::_refreshMotorAdjustment(){
motorAdjustment=map(analogRead(TRIM),0,1023,-30,30)/100.0;
}
void RobotMotorBoard::reportActionDone(){
setMode(MODE_SIMPLE);
messageOut.writeByte(COMMAND_ACTION_DONE);
messageOut.sendData();
}
RobotMotorBoard RobotMotor=RobotMotorBoard();

125
libraries/Robot_Motor/ArduinoRobotMotorBoard.h Normal file
View File

@ -0,0 +1,125 @@
#ifndef ArduinoRobot_h
#define ArduinoRobot_h
#include "EasyTransfer2.h"
#include "Multiplexer.h"
#include "LineFollow.h"
//#include "IRremote.h"
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
//Command code
#define COMMAND_SWITCH_MODE 0
#define COMMAND_RUN 10
#define COMMAND_MOTORS_STOP 11
#define COMMAND_ANALOG_WRITE 20
#define COMMAND_DIGITAL_WRITE 30
#define COMMAND_ANALOG_READ 40
#define COMMAND_ANALOG_READ_RE 41
#define COMMAND_DIGITAL_READ 50
#define COMMAND_DIGITAL_READ_RE 51
#define COMMAND_READ_IR 60
#define COMMAND_READ_IR_RE 61
#define COMMAND_ACTION_DONE 70
#define COMMAND_READ_TRIM 80
#define COMMAND_READ_TRIM_RE 81
#define COMMAND_PAUSE_MODE 90
#define COMMAND_LINE_FOLLOW_CONFIG 100
//component codename
#define CN_LEFT_MOTOR 0
#define CN_RIGHT_MOTOR 1
#define CN_IR 2
//motor board modes
#define MODE_SIMPLE 0
#define MODE_LINE_FOLLOW 1
#define MODE_ADJUST_MOTOR 2
#define MODE_IR_CONTROL 3
//bottom TKs, just for communication purpose
#define B_TK1 201
#define B_TK2 202
#define B_TK3 203
#define B_TK4 204
/*
A message structure will be:
switch mode (2):
byte COMMAND_SWITCH_MODE, byte mode
run (5):
byte COMMAND_RUN, int speedL, int speedR
analogWrite (3):
byte COMMAND_ANALOG_WRITE, byte codename, byte value;
digitalWrite (3):
byte COMMAND_DIGITAL_WRITE, byte codename, byte value;
analogRead (2):
byte COMMAND_ANALOG_READ, byte codename;
analogRead _return_ (4):
byte COMMAND_ANALOG_READ_RE, byte codename, int value;
digitalRead (2):
byte COMMAND_DIGITAL_READ, byte codename;
digitalRead _return_ (4):
byte COMMAND_DIGITAL_READ_RE, byte codename, int value;
read IR (1):
byte COMMAND_READ_IR;
read IR _return_ (9):
byte COMMAND_READ_IR_RE, int valueA, int valueB, int valueC, int valueD;
*/
class RobotMotorBoard:public LineFollow{
public:
RobotMotorBoard();
void begin();
void process();
void parseCommand();
int IRread(uint8_t num);
void setMode(uint8_t mode);
void pauseMode(bool onOff);
void motorsWrite(int speedL, int speedR);
void motorsWritePct(int speedLpct, int speedRpct);//write motor values in percentage
void motorsStop();
private:
float motorAdjustment;//-1.0 ~ 1.0, whether left is lowered or right is lowered
//convert codename to actual pins
uint8_t parseCodename(uint8_t codename);
uint8_t codenameToAPin(uint8_t codename);
void stopCurrentActions();
//void sendCommand(byte command,byte codename,int value);
void _analogWrite(uint8_t codename, int value);
void _digitalWrite(uint8_t codename, bool value);
void _analogRead(uint8_t codename);
void _digitalRead(uint8_t codename);
void _readIR();
void _readTrim();
void _refreshMotorAdjustment();
Multiplexer IRs;
uint8_t mode;
uint8_t isPaused;
EasyTransfer2 messageIn;
EasyTransfer2 messageOut;
//Line Following
void reportActionDone();
};
extern RobotMotorBoard RobotMotor;
#endif

152
libraries/Robot_Motor/EasyTransfer2.cpp Normal file
View File

@ -0,0 +1,152 @@
#include "EasyTransfer2.h"
//Captures address and size of struct
void EasyTransfer2::begin(HardwareSerial *theSerial){
_serial = theSerial;
//dynamic creation of rx parsing buffer in RAM
//rx_buffer = (uint8_t*) malloc(size);
resetData();
}
void EasyTransfer2::writeByte(uint8_t dat){
if(position<20)
data[position++]=dat;
size++;
}
void EasyTransfer2::writeInt(int dat){
if(position<19){
data[position++]=dat>>8;
data[position++]=dat;
size+=2;
}
}
uint8_t EasyTransfer2::readByte(){
if(position>=size)return 0;
return data[position++];
}
int EasyTransfer2::readInt(){
if(position+1>=size)return 0;
int dat_1=data[position++]<<8;
int dat_2=data[position++];
int dat= dat_1 | dat_2;
return dat;
}
void EasyTransfer2::resetData(){
for(int i=0;i<20;i++){
data[i]=0;
}
size=0;
position=0;
}
//Sends out struct in binary, with header, length info and checksum
void EasyTransfer2::sendData(){
uint8_t CS = size;
_serial->write(0x06);
_serial->write(0x85);
_serial->write(size);
for(int i = 0; i<size; i++){
CS^=*(data+i);
_serial->write(*(data+i));
//Serial.print(*(data+i));
//Serial.print(",");
}
//Serial.println("");
_serial->write(CS);
resetData();
}
boolean EasyTransfer2::receiveData(){
//start off by looking for the header bytes. If they were already found in a previous call, skip it.
if(rx_len == 0){
//this size check may be redundant due to the size check below, but for now I'll leave it the way it is.
if(_serial->available() >= 3){
//this will block until a 0x06 is found or buffer size becomes less then 3.
while(_serial->read() != 0x06) {
//This will trash any preamble junk in the serial buffer
//but we need to make sure there is enough in the buffer to process while we trash the rest
//if the buffer becomes too empty, we will escape and try again on the next call
if(_serial->available() < 3)
return false;
}
//Serial.println("head");
if (_serial->read() == 0x85){
rx_len = _serial->read();
//Serial.print("rx_len:");
//Serial.println(rx_len);
resetData();
//make sure the binary structs on both Arduinos are the same size.
/*if(rx_len != size){
rx_len = 0;
return false;
}*/
}
}
//Serial.println("nothing");
}
//we get here if we already found the header bytes, the struct size matched what we know, and now we are byte aligned.
if(rx_len != 0){
while(_serial->available() && rx_array_inx <= rx_len){
data[rx_array_inx++] = _serial->read();
}
if(rx_len == (rx_array_inx-1)){
//seem to have got whole message
//last uint8_t is CS
calc_CS = rx_len;
//Serial.print("len:");
//Serial.println(rx_len);
for (int i = 0; i<rx_len; i++){
calc_CS^=data[i];
//Serial.print("m");
//Serial.print(data[i]);
//Serial.print(",");
}
//Serial.println();
//Serial.print(data[rx_array_inx-1]);
//Serial.print(" ");
//Serial.println(calc_CS);
if(calc_CS == data[rx_array_inx-1]){//CS good
//resetData();
//memcpy(data,d,rx_len);
for(int i=0;i<20;i++){
//Serial.print(data[i]);
//Serial.print(",");
}
//Serial.println("");
size=rx_len;
rx_len = 0;
rx_array_inx = 0;
return true;
}
else{
//Serial.println("CS");
resetData();
//failed checksum, need to clear this out anyway
rx_len = 0;
rx_array_inx = 0;
return false;
}
}
}
//Serial.print(rx_len);
//Serial.print(" ");
//Serial.print(rx_array_inx);
//Serial.print(" ");
//Serial.println("Short");
return false;
}

76
libraries/Robot_Motor/EasyTransfer2.h Normal file
View File

@ -0,0 +1,76 @@
/******************************************************************
* EasyTransfer Arduino Library
* details and example sketch:
* http://www.billporter.info/easytransfer-arduino-library/
*
* Brought to you by:
* Bill Porter
* www.billporter.info
*
* See Readme for other info and version history
*
*
*This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
<http://www.gnu.org/licenses/>
*
*This work is licensed under the Creative Commons Attribution-ShareAlike 3.0 Unported License.
*To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/3.0/ or
*send a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
******************************************************************/
#ifndef EasyTransfer2_h
#define EasyTransfer2_h
//make it a little prettier on the front end.
#define details(name) (byte*)&name,sizeof(name)
//Not neccessary, but just in case.
#if ARDUINO > 22
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include "HardwareSerial.h"
//#include <NewSoftSerial.h>
#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <avr/io.h>
class EasyTransfer2 {
public:
void begin(HardwareSerial *theSerial);
//void begin(uint8_t *, uint8_t, NewSoftSerial *theSerial);
void sendData();
boolean receiveData();
void writeByte(uint8_t dat);
void writeInt(int dat);
uint8_t readByte();
int readInt();
private:
HardwareSerial *_serial;
void resetData();
uint8_t data[20]; //data storage, for both read and send
uint8_t position;
uint8_t size; //size of data in bytes. Both for read and send
//uint8_t * address; //address of struct
//uint8_t size; //size of struct
//uint8_t * rx_buffer; //address for temporary storage and parsing buffer
//uint8_t rx_buffer[20];
uint8_t rx_array_inx; //index for RX parsing buffer
uint8_t rx_len; //RX packet length according to the packet
uint8_t calc_CS; //calculated Chacksum
};
#endif

40
libraries/Robot_Motor/LineFollow.h Normal file
View File

@ -0,0 +1,40 @@
#ifndef LINE_FOLLOW_H
#define LINE_FOLLOW_H
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class LineFollow{
public:
LineFollow();
void calibIRs();
void runLineFollow();
void config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime);
//These are all pure virtual functions, pure VF needs pure specifier "=0"
//virtual void motorsWrite(int speedL, int speedR)=0;
virtual void motorsWritePct(int speedLpct, int speedRpct)=0;
virtual void motorsStop()=0;
virtual int IRread(uint8_t num)=0;
protected:
virtual void reportActionDone()=0;
private:
void doCalibration(int speedPct, int time);
void ajusta_niveles();
uint8_t KP;
uint8_t KD;
uint8_t robotSpeed; //percentage
uint8_t intergrationTime;
int lectura_sensor[5], last_error, acu;
int sensor_blanco[5];
int sensor_negro[5];
};
#endif

37
libraries/Robot_Motor/Multiplexer.cpp Normal file
View File

@ -0,0 +1,37 @@
#include "Multiplexer.h"
void Multiplexer::begin(uint8_t* selectors, uint8_t Z, uint8_t length){
for(uint8_t i=0;i<length;i++){
this->selectors[i]=selectors[i];
pinMode(selectors[i],OUTPUT);
}
this->length=length;
this->pin_Z=Z;
pinMode(pin_Z,INPUT);
}
void Multiplexer::selectPin(uint8_t num){
for(uint8_t i=0;i<length;i++){
//Serial.print(bitRead(num,i));
digitalWrite(selectors[i],bitRead(num,i));
}
//Serial.println("");
}
int Multiplexer::getAnalogValue(){
return analogRead(pin_Z);
}
bool Multiplexer::getDigitalValue(){
return digitalRead(pin_Z);
}
int Multiplexer::getAnalogValueAt(uint8_t num){
selectPin(num);
return getAnalogValue();
}
bool Multiplexer::getDigitalValueAt(uint8_t num){
selectPin(num);
return getDigitalValue();
}

24
libraries/Robot_Motor/Multiplexer.h Normal file
View File

@ -0,0 +1,24 @@
#ifndef Multiplexer_h
#define Multiplexer_h
#if ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
class Multiplexer{
public:
void begin(uint8_t* selectors, uint8_t Z, uint8_t length);
void selectPin(uint8_t num);
int getAnalogValue();
int getAnalogValueAt(uint8_t num);
bool getDigitalValue();
bool getDigitalValueAt(uint8_t num);
private:
uint8_t selectors[4];
uint8_t pin_Z;
uint8_t length;
};
#endif

26
libraries/Robot_Motor/examples/Robot_IR_Array_Test/Robot_IR_Array_Test.ino Normal file
View File

@ -0,0 +1,26 @@
/* Motor Board IR Array Test
This example of the Arduno robot's motor board returns the
values read fron the 5 infrared sendors on the bottom of
the robot.
*/
// include the motor board header
#include <ArduinoRobotMotorBoard.h>
String bar; // string for storing the informaton
void setup(){
// start serial communication
Serial.begin(9600);
// initialize the library
RobotMotor.begin();
}
void loop(){
bar=String(""); // empty the string
// read the sensors and add them to the string
bar=bar+RobotMotor.readIR(0)+' '+RobotMotor.readIR(1)+' '+RobotMotor.readIR(2)+' '+RobotMotor.readIR(3)+' '+RobotMotor.readIR(4);
// print out the values
Serial.println(bar);
delay(100);
}

18
libraries/Robot_Motor/examples/Robot_Motor_Core/Robot_Motor_Core.ino Normal file
View File

@ -0,0 +1,18 @@
/* Motor Core
This code for the Arduino Robot's motor board
is the stock firmware. program the motor board with
this sketch whenever you want to return the motor
board to its default state.
*/
#include <ArduinoRobotMotorBoard.h>
void setup(){
RobotMotor.begin();
}
void loop(){
RobotMotor.parseCommand();
RobotMotor.process();
}

152
libraries/Robot_Motor/lineFollow.cpp Normal file
View File

@ -0,0 +1,152 @@
//#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;
}
}