Diff: PC_Comms/PC_Comms.cpp

Revision:

27:d032d8fd4b45

Parent:

26:78282794606d

--- a/PC_Comms/PC_Comms.cpp	Mon Mar 07 18:53:33 2016 +0000
+++ b/PC_Comms/PC_Comms.cpp	Thu Mar 10 14:39:28 2016 +0000
@@ -19,8 +19,9 @@
     Serial _PCbus(USBTX, USBRX);
     AX12 my_AX12(ax12_bus, _PCbus, p29, 1);
     AX12 my_AX12_2(ax12_bus, _PCbus, p29, 2);
+    AX12 my_AX12_3(ax12_bus, _PCbus, p29, 3);
     
-    int angle2;
+    int angle3;
     char _servo;
     
 
@@ -92,9 +93,8 @@
                     angle[0] = Command[4];
                     angle[1] = Command[5];
                     angle[2] = Command[6];
-                    angle2 = atoi(angle);                 
-                    my_AX12.SetGoal(angle2, '00');
-                    _servo = '1';
+                    angle3 = atoi(angle);                 
+                    my_AX12.SetGoal(angle3, '00');
                     break;
                 
                 case '1':
@@ -102,9 +102,17 @@
                     angle1[0] = Command[4];
                     angle1[1] = Command[5];
                     angle1[2] = Command[6];
-                    angle2 = atoi(angle1);                
-                    my_AX12_2.SetGoal(angle2, 'ff');
-                    _servo = '2';
+                    angle3 = atoi(angle1);                
+                    my_AX12_2.SetGoal(angle3, 'ff');
+                    break;
+                
+                case '2':
+                    char angle2[3];
+                    angle2[0] = Command[4];
+                    angle2[1] = Command[5];
+                    angle2[2] = Command[6];
+                    angle3 = atoi(angle2);                
+                    my_AX12_2.SetGoal(angle3, 'ff');
                     break;
                 default:
                     break;
@@ -123,7 +131,7 @@
     _returnToPC[0] = 'S';
     _returnToPC[1] = 'D';
     _returnToPC[2] = '0';
-    _returnToPC[3] = _servo;
+    _returnToPC[3] = '1';
     
     if(load_data >= 1000){
     _PCbus.printf("%s%d\n",_returnToPC, load_data);
@@ -135,6 +143,28 @@
         _PCbus.printf("%s000%d\n", _returnToPC, load_data);
     }
 }
+ 
+ void PC_Comms::PC_WriteLoad_2(char _servo){   
+       
+    char _returnToPC_1[4];
+    int load_data_1;
+    
+    load_data_1 = my_AX12_2.GetLoad();
+    _returnToPC_1[0] = 'S';
+    _returnToPC_1[1] = 'D';
+    _returnToPC_1[2] = '0';
+    _returnToPC_1[3] = '2';
+    
+    if(load_data_1 >= 1000){
+    _PCbus.printf("%s%d\n",_returnToPC_1, load_data_1);
+    }else if(load_data_1 < 1000 && load_data_1 >= 100){
+        _PCbus.printf("%s0%d\n", _returnToPC_1, load_data_1);
+    }else if(load_data_1 < 100 && load_data_1 >= 10){
+        _PCbus.printf("%s00%d\n", _returnToPC_1, load_data_1);
+    }else if(load_data_1 < 10){
+        _PCbus.printf("%s000%d\n", _returnToPC_1, load_data_1);
+    }
+}
 
 void PC_Comms::PC_WritePosition(char _servo){
     
@@ -148,15 +178,38 @@
     _returnToPC[3] = _servo;
     
      
-//     if(pos_data >= 1000){
+     if(pos_data >= 1000){
     _PCbus.printf("%s%i\n",_returnToPC,pos_data);
-//    }else if(pos_data < 1000 && pos_data >= 100){
-//        _PCbus.printf("%s0%i\n", _returnToPC, pos_data);
-//    }else if(pos_data < 100 && pos_data >= 10){
-//        _PCbus.printf("%s00%i\n", _returnToPC, pos_data);
-//    }else{
-//        _PCbus.printf("%s000%i\n", _returnToPC, pos_data);
-//    }
+    }else if(pos_data < 1000 && pos_data >= 100){
+        _PCbus.printf("%s0%i\n", _returnToPC, pos_data);
+    }else if(pos_data < 100 && pos_data >= 10){
+        _PCbus.printf("%s00%i\n", _returnToPC, pos_data);
+    }else{
+        _PCbus.printf("%s000%i\n", _returnToPC, pos_data);
+    }
+}
+
+void PC_Comms::PC_WriteVoltage(char _servo){
+    
+    char _returnToPC[4];
+    int volts_data;
+
+    volts_data = my_AX12.GetVoltage();
+    _returnToPC[0] = 'S';
+    _returnToPC[1] = 'D';
+    _returnToPC[2] = '1';
+    _returnToPC[3] = _servo;
+    
+     
+     if(volts_data >= 1000){
+    _PCbus.printf("%s%i\n",_returnToPC,volts_data);
+    }else if(volts_data < 1000 && volts_data >= 100){
+        _PCbus.printf("%s0%i\n", _returnToPC, volts_data);
+    }else if(volts_data < 100 && volts_data >= 10){
+        _PCbus.printf("%s00%i\n", _returnToPC, volts_data);
+    }else{
+        _PCbus.printf("%s000%i\n", _returnToPC, volts_data);
+    }
 }
 
 void PC_Comms::PC_Human(){
@@ -165,7 +218,7 @@
     
     load_data = my_AX12.GetLoad();
     
-    if(load_data >= 1070&& angle2<=270 && angle2>=70 && 1!= my_AX12.isMoving()){      
+    if(load_data >= 1070 && 1!= my_AX12.isMoving()){      
 
         my_AX12.TorqueEnable(0);
 }