1
0
mirror of https://github.com/arduino/Arduino.git synced 2024-12-10 21:24:12 +01:00
Arduino/libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp

269 lines
5.9 KiB
C++

#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(speedR>0){
analogWrite(IN_A1,speedR);
analogWrite(IN_A2,0);
}else{
analogWrite(IN_A1,0);
analogWrite(IN_A2,-speedR);
}
if(speedL>0){
analogWrite(IN_B1,speedL);
analogWrite(IN_B2,0);
}else{
analogWrite(IN_B1,0);
analogWrite(IN_B2,-speedL);
}
}
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();