Merge branch 'merge-1.0.5' into ide-1.5.x-discovery

1 files changed, 265 insertions, 0 deletions

diff --git a/libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp b/libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp
new file mode 100644
index 0000000..7740a06
--- /dev/null
+++ b/libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp
@@ -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();
\ No newline at end of file