mirror of
https://github.com/arduino/Arduino.git
synced 2024-12-10 21:24:12 +01:00
269 lines
5.9 KiB
C++
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(); |