Diff: AX12.cpp

Revision:

23:c1c20aee64a0

Parent:

21:ab49231d1479

Child:

24:462afd19d12e

--- a/AX12.cpp	Mon Feb 15 15:55:27 2016 +0000
+++ b/AX12.cpp	Wed Feb 17 11:55:04 2016 +0000
@@ -10,25 +10,56 @@
     _dir = TRANSMIT;
 }
 
-int AX12::SetGoal(int degrees, int flags){
-    
-    char reg_flag = 0;
-    char data[2];
-    
-    if (flags == 0x2) {
-        reg_flag = 1;
-    }
+int AX12::SetGoal(int degrees, int speed){
       
     short goal = (1023*degrees)/300;
+    char data[4];
     data[0] = goal & 0xff;
     data[1] = goal >> 8;
+    data[2] = 0x00;
+    data[3] = 0x00;
+
     
-    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data);
+    
+    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 4, data);
+    
+    return (rVal);
+}
+
+// Set the mode of the servo
+//  0 = Positional (0-300 degrees)
+//  1 = Rotational -1 to 1 speed
 
-if (flags == 1){
-    while (isMoving()) {}
+int AX12::SetMode(int mode) {
+ 
+    if (mode == 1) {
+        
+        SetCWLimit(0);
+        SetCCWLimit(0);
+        SetCRSpeed(0.0);
+    } else {
+        SetCWLimit(70);
+        SetCCWLimit(270);
+        SetCRSpeed(0.0);
     }
-    return(rVal);
+    return(0);
+}
+
+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
+    }
+ 
+    // write the packet, return the error code
+    return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data)); 
 }
 
 int AX12::SetCWLimit (int degrees) {
@@ -58,6 +89,28 @@
     return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
 }
 
+int AX12::SetCRSpeed(float speed) {
+ 
+    // bit 10     = direction, 0 = CCW, 1=CW
+    // bits 9-0   = Speed
+    char data[2];
+ 
+    int goal = (0x3ff * abs(speed));
+ 
+    // Set direction CW if we have a negative speed
+    if (speed < 0) {
+        goal |= (0x1 << 10);
+    }
+ 
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+ 
+    // write the packet, return the error code
+    int rVal = write(_ID, 0x20, 2, data);
+ 
+    return(rVal);
+}
+
 int AX12::isMoving(void) {
  
     char data[1];