--- /dev/null
+package lejos.nxt;\r
+\r
+import lejos.nxt.*;\r
+\r
+\r
+/**\r
+ * LServo, Lattebox Servo, is a abstraction to model any Servo connected to\r
+ * LSC, Lattebox Servo Controller. \r
+ * \r
+ * @author Juan Antonio Brenha Moral\r
+ */\r
+public class LServo extends I2CSensor{\r
+ private String servoName = "";//String to describe any Servo connected to LSC\r
+ private int angle;//Angle\r
+ private int speed;//Not implemented yet.\r
+ private int min_angle;\r
+ private int max_angle;\r
+ \r
+ //Servo ID\r
+ private SensorPort portConnected;//What\r
+ private byte SPI_PORT;//What SPI Port is connected LSC\r
+ private int servo;\r
+ \r
+ //I2C\r
+ int I2C_Response;\r
+ private byte[] bufReadResponse;\r
+ private byte h_byte;\r
+ private byte l_byte;\r
+ \r
+ /**\r
+ * Constructor\r
+ * \r
+ * @param port\r
+ * @param location\r
+ * @param servoName\r
+ * @param SPI_PORT\r
+ * \r
+ * Author: Juan Antonio Brenha Moral \r
+ */\r
+ public LServo(SensorPort port, int location, String servoName, byte SPI_PORT){\r
+ super(port);\r
+ this.servoName = servoName;\r
+ \r
+ this.SPI_PORT = SPI_PORT;\r
+ \r
+ bufReadResponse = new byte[8]; \r
+ this.setAddress((int) NXTe.NXTE_ADDRESS);\r
+ }\r
+ \r
+ /**\r
+ * Method to setAngle in Servo. \r
+ * \r
+ * Note:\r
+ * In next version, I will delete servo parameter, \r
+ * because the object should know servo id.\r
+ * \r
+ * @param servo\r
+ * @param angle\r
+ * @throws Exception\r
+ * \r
+ * Author: Juan Antonio Brenha Moral\r
+ */\r
+ public void setAngle(int servo,int angle) throws Exception{\r
+ h_byte = (byte)(0x80 | ((servo<<3) | (angle >>8)));\r
+ l_byte = (byte)angle;\r
+ \r
+ //High Byte Write\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, h_byte);\r
+\r
+ //Low Byte Write\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, l_byte);\r
+ }\r
+ \r
+ /**\r
+ * \r
+ * Method to know the angle\r
+ * \r
+ * Note:\r
+ * In next version, I will delete servo parameter, \r
+ * because the object should know servo id.\r
+ *\r
+ * @param Servo\r
+ * @return angle\r
+ * @throws Exception\r
+ */\r
+ public int getAngle(int Servo) throws Exception{\r
+ \r
+ //Write OP Code\r
+ h_byte = (byte)(Servo << 3);\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, h_byte);\r
+ \r
+ //Read High Byte\r
+ //I2CBytes(IN_3, bufReadValue, buflen, bufReadResponse);\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, (byte)0x00); \r
+ I2C_Response = this.getData((int)this.SPI_PORT, bufReadResponse, 1);\r
+ \r
+ h_byte = bufReadResponse[0];\r
+ \r
+ //Read Low Byte\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, (byte)0x00);\r
+ I2C_Response = this.getData((int)this.SPI_PORT, bufReadResponse, 1);\r
+ l_byte = bufReadResponse[0];\r
+ \r
+ return ((h_byte & 0x07 ) << 8) + (l_byte & 0x00000000FF);\r
+ }\r
+ \r
+ \r
+ /**\r
+ * \r
+ * public method to know internal information about \r
+ * if the servo is moving\r
+ * \r
+ * @return motion\r
+ * @throws Exceptionl\r
+ */\r
+ public int readMotion() throws Exception{\r
+ int motion = -1;\r
+ \r
+ //Write OP Code\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, (byte)0x68);\r
+ \r
+ //Read High Byte\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, (byte)0x00); \r
+ I2C_Response = this.getData((int)this.SPI_PORT, bufReadResponse, 1);\r
+ h_byte = bufReadResponse[0];\r
+\r
+ //Read Low Byte\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, (byte)0x00); \r
+ I2C_Response = this.getData((int)this.SPI_PORT, bufReadResponse, 1);\r
+ l_byte = bufReadResponse[0];\r
+ \r
+ if(l_byte == 0xFF){\r
+ motion = ((h_byte & 0x07 ) << 8) + 255;\r
+ }else{\r
+ motion = ((h_byte & 0x07 ) << 8)|(l_byte&0xFF);\r
+ }\r
+ return motion;\r
+ }\r
+ \r
+ /**\r
+ * Method to know if Servo is moving to a determinated angle\r
+ * \r
+ * @return true iff the serv is moving\r
+ * @throws Exception\r
+ */\r
+ public boolean isMoving() throws Exception{\r
+ boolean flag = false;\r
+ if(readMotion() != 0){\r
+ flag = true;\r
+ }\r
+ return flag;\r
+ }\r
+ \r
+ /**\r
+ * Set a delay in Servo\r
+ * \r
+ * Note:\r
+ * In next version, I will delete servo parameter, \r
+ * because the object should know servo id.\r
+ * \r
+ * @param Servo\r
+ * @param delay\r
+ */\r
+ public void setDelay(int Servo,int delay){\r
+ h_byte = (byte)0xF0;\r
+ l_byte = (byte)(((Servo)<<4) + delay);\r
+ \r
+ I2C_Response = this.sendData((int)this.SPI_PORT, (byte)h_byte);\r
+ I2C_Response = this.sendData((int)this.SPI_PORT, (byte)l_byte);\r
+ }\r
+ \r
+ /**\r
+ * Set Minimal angle. Useful method to calibrate a Servo\r
+ * \r
+ * @param minAngle\r
+ */\r
+ public void setMinAngle(int minAngle){\r
+ this.min_angle = minAngle;\r
+ }\r
+\r
+ /**\r
+ * Set Maximum angle. Useful method to calibrate a Servo\r
+ * \r
+ * @param maxAngle\r
+ */ \r
+ public void setMaxAngle(int maxAngle){\r
+ this.max_angle = maxAngle;\r
+ } \r
+\r
+ /**\r
+ * Method to set minimal angle\r
+ */ \r
+ public void goToMinAngle() throws Exception{\r
+ this.setAngle(1, this.min_angle);\r
+ }\r
+\r
+ /**\r
+ * Method to set maximum angle\r
+ */ \r
+ public void goToMaxAngle() throws Exception{\r
+ this.setAngle(1, this.max_angle); \r
+ }\r
+\r
+ /**\r
+ * Method to set medium angle\r
+ */ \r
+ public void goToMiddleAngle() throws Exception{\r
+ float middle = (this.min_angle + this.max_angle) / 2;\r
+ \r
+ this.setAngle(1, Math.round(middle)); \r
+ }\r
+\r
+ /**\r
+ * Get servo name\r
+ */ \r
+ public String getName(){\r
+ return this.servoName;\r
+ }\r
+}\r
OSDN Git Service
