Diff: AX12.cpp

Revision:

24:462afd19d12e

Parent:

23:c1c20aee64a0

Child:

25:d345ef4cb2b5

--- a/AX12.cpp	Wed Feb 17 11:55:04 2016 +0000
+++ b/AX12.cpp	Thu Mar 03 12:07:25 2016 +0000
@@ -48,18 +48,25 @@
 int AX12::SetTorqueLimit (int torque) {
  
     char data[2];
- 
-    if(torque ==1){
-    data[0] = 0xff; // bottom 8 bits
-    data[1] = 0xff;   // top 8 bits
-    }else{
-        
-    data[0] = 0x00; // bottom 8 bits
-    data[1] = 0x00;   // top 8 bits
-    }
+    int set = (torque*1023)/10;
+
+    data[0] = set && 0xff; // bottom 8 bits
+    data[1] = set >> 8;   // top 8 bits
+    
  
     // write the packet, return the error code
     return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data)); 
+    
+}
+
+int AX12::TorqueEnable (int enable) {
+ 
+    char data[1];
+    
+    data[0] = enable;
+ 
+    return (write(_ID, AX12_REG_TORQUE_ENABLE, 1, data)); 
+    
 }
 
 int AX12::SetCWLimit (int degrees) {
@@ -130,10 +137,10 @@
     char data[2];
     
     int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
-    int position = data[0] + (data[1] << 8);
-//    int angle  = (position * 300)/1023;
-//    (1023 * degrees) / 300
-    return (position);
+    short position = data[0] + (data[1] << 8);
+    float angle  = ((position * 300)/1023);
+
+    return (angle);
 }
 
 int AX12::read(int ID, int start, int bytes, char* data){