/*
* File : MotorController.java
*
* Copyright (C) 2008 Steliana Vatau <steliana.vatau@jquadrobot.org>
*
* www.jquadrobot.org
*
* This file is part of JQuadRobot.
*
* JQuadRobot 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.
*
* JQuadRobot 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.
*
* You should have received a copy of the GNU General Public License
* along with JQuadRobot. If not, see <http://www.gnu.org/licenses/>.
*
*/
package org.jquadrobot.communication;
import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;
import java.io.IOException;
import java.io.OutputStream;
/**
* This class command a motor using serial port.
*
* @version 1.4
* @author Steliana Vatau <steliana.vatau@jquadrobot.org>
*/
public class MotorController implements HomePosition{
String comPort = "COM1";
SerialPort serialPort;
OutputStream out;
MotorRotLimit mrt = new MotorRotLimit();
String os;
public MotorController(boolean debug){
getOSName();
if(!debug){
try {
connect(comPort);
} catch (Exception e) {
e.printStackTrace();
}
}
}
/*
* if the OS is a MAC OS.
*/
void getOSName(){
String osName = System.getProperties().getProperty( "os.name" ); // get the OS name;
if(osName.contains("Mac")){
os = "mac";
comPort = "/dev/tty.usbserial-A7006fQf";
}
}
void connect ( String portName ) throws Exception
{
CommPortIdentifier portIdentifier = CommPortIdentifier.getPortIdentifier(portName);
if ( portIdentifier.isCurrentlyOwned() )
{
System.out.println("Error: Port is currently in use");
}
else
{
CommPort commPort = portIdentifier.open(this.getClass().getName(),2000);
if ( commPort instanceof SerialPort )
{
serialPort = (SerialPort) commPort;
serialPort.setSerialPortParams(115200,SerialPort.DATABITS_8,SerialPort.STOPBITS_1,SerialPort.PARITY_NONE);
// InputStream in = serialPort.getInputStream();
out = serialPort.getOutputStream();
}
else
{
System.out.println("No serial port detected.");
}
}
}
public void newPosition(int motor, int value){
if(value > mrt.rotMin[motor] && value < mrt.rotMax[motor]){ // check if the desired value is betwen limits;
if(motor > 5){
motor = motor+10;
}
String command = "#"+motor+" P"+value+"\r";
System.out.println("command to port > "+command);
byte[] commandInBytes = command.getBytes();
try {
out.write(commandInBytes);
} catch (IOException e) {
e.printStackTrace();
}
}
}
/*
* run commands from graph;
*/
public void sendCommand(String command){
//System.out.println("sent on serial= "+command);
//String command40A = newPowerSource(command);
//byte[] commandInBytes = command40A.getBytes();
byte[] commandInBytes = command.getBytes();
try {
out.write(commandInBytes);
} catch (IOException e) {
e.printStackTrace();
}
}
public String newPowerSource(String cmd40A){
String oldCmd = cmd40A;
String resultCmd = cmd40A;
String subCmd = oldCmd.substring(1, 3);
String tempCmd = oldCmd.substring(3).trim();
Integer mnr = (Integer.valueOf(subCmd.trim())).intValue();
if(mnr > 5){
mnr = mnr+10;
resultCmd = "#"+mnr+" "+tempCmd;
}
System.out.println("new command = "+resultCmd);
return resultCmd;
}
public void homePositionOld(){
String command0 = "#0 P600 #1 P600 #2 P600 #3 P600 #4 P600 #5 P600 #6 P600 #7 P600 #8 P600 #9 P600 #10 P600 #11 P600 \r";
byte[] commandInBytes0 = command0.getBytes();
try {
out.write(commandInBytes0);
} catch (IOException e) {
e.printStackTrace();
}
String command = "#0 P"+HPM_1+" #1 P"+HPM_2+" #2 P"+HPM_3+" #3 P"+HPM_4
+" #4 P"+HPM_5+" #5 P"+HPM_6+" #6 P"+HPM_7+" #7 P"+HPM_8+" #8 P"+HPM_9
+" #9 P"+HPM_10+" #10 P"+HPM_11+" #11 P"+HPM_12+" T4000 "+"\r"; // perform home position in 2s;
byte[] commandInBytes = command.getBytes();
try {
out.write(commandInBytes);
} catch (IOException e) {
e.printStackTrace();
}
}
public void resetPositionOld(){
String command = "#0 P"+HPM_1+" #1 P"+HPM_2+" #2 P"+HPM_3+" #3 P"+HPM_4
+" #4 P"+HPM_5+" #5 P"+HPM_6+" #6 P"+HPM_7+" #7 P"+HPM_8+" #8 P"+HPM_9
+" #9 P"+HPM_10+" #10 P"+HPM_11+" #11 P"+HPM_12+" T4000 "+"\r"; // perform home position in 2s;
byte[] commandInBytes = command.getBytes();
try {
out.write(commandInBytes);
} catch (IOException e) {
e.printStackTrace();
}
}
public void exitHomePositionOld(){
String command0 = "#0 P600 #1 P600 #2 P600 #3 P600 #4 P600 #5 P600 #6 P600 #7 P600 #8 P600 #9 P600 #10 P600 #11 P600 T4000 \r";
byte[] commandInBytes0 = command0.getBytes();
try {
out.write(commandInBytes0);
} catch (IOException e) {
e.printStackTrace();
}
}
public void homePosition(){
String[] command0= new String[3];
command0[0]= "#0 P600 #3 P600 #16 P600 #19 P600 \r";
command0[1]= "#1 P600 #4 P600 #17 P600 #20 P600 \r";
command0[2]= "#2 P600 #5 P600 #18 P600 #21 P600 \r";
for(int i=0; i<3; i++){
byte[] commandInBytes0 = command0[i].getBytes();
try {
out.write(commandInBytes0);
} catch (IOException e) {
e.printStackTrace();
}
long it = System.currentTimeMillis();
while(System.currentTimeMillis()-it > 100){
// delay 100 ms
}
}
String[] command= new String[3];
command[0]= "#0 P"+HPM_1+" #3 P"+HPM_4+" #16 P"+HPM_7+" #19 P"+HPM_10+" T1000 "+"\r";
command[1]= "#1 P"+HPM_2+" #4 P"+HPM_5+" #17 P"+HPM_8+" #20 P"+HPM_11+" T1000 "+"\r";
command[2]= "#2 P"+HPM_3+" #5 P"+HPM_6+" #18 P"+HPM_9+" #21 P"+HPM_12+" T1000 "+"\r";
for(int i=0; i<3; i++){
byte[] commandInBytes = command[i].getBytes();
try {
out.write(commandInBytes);
} catch (IOException e) {
e.printStackTrace();
}
long itt = System.currentTimeMillis();
while(System.currentTimeMillis()-itt > 1100){
// delay 1100 ms
}
}
}
public void resetPosition(){
String[] command= new String[3];
command[0]= "#0 P"+HPM_1+" #3 P"+HPM_4+" #16 P"+HPM_7+" #19 P"+HPM_10+" T1000 "+"\r";
command[1]= "#1 P"+HPM_2+" #4 P"+HPM_5+" #17 P"+HPM_8+" #20 P"+HPM_11+" T1000 "+"\r";
command[2]= "#2 P"+HPM_3+" #5 P"+HPM_6+" #18 P"+HPM_9+" #21 P"+HPM_12+" T1000 "+"\r";
for(int i=0; i<3; i++){
byte[] commandInBytes = command[i].getBytes();
try {
out.write(commandInBytes);
} catch (IOException e) {
e.printStackTrace();
}
long itt = System.currentTimeMillis();
while(System.currentTimeMillis()-itt > 1100){
// delay 1100 ms
}
}
}
public void exitHomePosition(){
String[] command0= new String[3];
command0[0]= "#0 P600 #3 P600 #16 P600 #19 P600 T1000 \r";
command0[1]= "#1 P600 #4 P600 #17 P600 #20 P600 T1000 \r";
command0[2]= "#2 P600 #5 P600 #18 P600 #21 P600 T1000 \r";
for(int i=0; i<3; i++){
byte[] commandInBytes0 = command0[i].getBytes();
try {
out.write(commandInBytes0);
} catch (IOException e) {
e.printStackTrace();
}
long it = System.currentTimeMillis();
while(System.currentTimeMillis()-it > 1100){
// delay 1100 ms
}
}
}
}