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

Robot_Motor library to the 1.5 format

Browse Source
This commit is contained in:
Fede85
2013-09-10 17:21:47 +02:00
parent a9cba03922
commit 3a4695fd09
9 changed files with 10 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

269
libraries/Robot_Motor/src/ArduinoRobotMotorBoard.cpp Normal file
View File

@ -0,0 +1,269 @@
#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){
return _IRread(num-1); //To make consistant with the pins labeled on the board
}
int RobotMotorBoard::_IRread(uint8_t num){
IRs.selectPin(num);
return IRs.getAnalogValue();
}
void RobotMotorBoard::_readIR(){
int value;
messageOut.writeByte(COMMAND_READ_IR_RE);
for(int i=0;i<5;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();

126
libraries/Robot_Motor/src/ArduinoRobotMotorBoard.h Normal file
View File

@ -0,0 +1,126 @@
#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);
int _IRread(uint8_t num);
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/src/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/src/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/src/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/src/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/src/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

152
libraries/Robot_Motor/src/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;
}
}