Diff: Comm/Comm.cpp

Revision:

9:1d9b24d7ac77

Parent:

8:1a3a69fecedf

Child:

10:769cc457c3a4

--- a/Comm/Comm.cpp	Thu Dec 03 23:55:44 2015 +0000
+++ b/Comm/Comm.cpp	Sat Dec 05 00:40:42 2015 +0000
@@ -1,11 +1,10 @@
 #include "Comm.h"
 
     
-Comm::Comm(CommDelegate *gains, CommDelegate *target):_pc(USBTX, USBRX), _json(&_pc)
+Comm::Comm(CommDelegate *controls):_pc(USBTX, USBRX), _json(&_pc)
 {
     _pc.baud(115200);
-    _gains = gains;
-    _target = target;
+    _controls = controls;
 }
 
 void Comm::openGripper1(Arguments* input, Reply* output){
@@ -38,28 +37,35 @@
     _json.close();
 }
 
-void Comm::setGains(Arguments* input, Reply* output){
-    if (input->argc < _gains->numGains()){
+void Comm::setSwingUpK(Arguments* input, Reply* output){
+    if (input->argc < 1){
         throwNotEnoughArgsError();
         return;
     }
-    float k1 = input->getArg<float>(); 
-    float d1 = input->getArg<float>(); 
-    float k2 = input->getArg<float>();  
-    float d2 = input->getArg<float>(); 
-    _gains->setGains(k1, d1, k2, d2);
+    float k = input->getArg<float>(); 
+    _controls->setSwingUpK(k);
+    printGains();
+}
+
+void Comm::setSwingUpD(Arguments* input, Reply* output){
+    if (input->argc < 1){
+        throwNotEnoughArgsError();
+        return;
+    }
+    float d = input->getArg<float>(); 
+    _controls->setSwingUpD(d);
     printGains();
 }
 
 void Comm::printGains(){
     _json.open();
-    _json.print("k1", _gains->getK1());
+    _json.print("swingUpK", _controls->getSwingUpK());
     _json.sepItem();
-    _json.print("d1", _gains->getD1());
+    _json.print("swingUpD", _controls->getSwingUpD());
     _json.sepItem();
-    _json.print("k2", _gains->getK2());
+    _json.print("k2", _controls->getK2());
     _json.sepItem();
-    _json.print("d2", _gains->getD1());
+    _json.print("d2", _controls->getD2());
     _json.close();
 }
 
@@ -69,23 +75,33 @@
         return;
     }
     int targetPosition = input->getArg<int>(); 
-    _target->setPosition(targetPosition);
+    _controls->setTargetPosition(targetPosition);
     printTarget();
 }
 
 void Comm::printTarget(){
     _json.open();
-    _json.print("target", _target->getPosition());
+    _json.print("target", _controls->getTargetPosition());
     _json.close();
 }
 
-void Comm::printPosition(CommDelegate *controls){
+void Comm::setTorque(Arguments * input, Reply * output){
+    if (input->argc < 1){
+        throwNotEnoughArgsError();
+        return;
+    }
+    float torque = atof(input->argv[0]);
+//    _pc.printf("%f", torque);
+    _controls->setTorque(torque);
+}
+
+void Comm::printPosition(){
     _json.open();
-    _json.print("th1", controls->getTheta1());
+    _json.print("th1", _controls->getTheta1());
+    _json.sepItem();
+//    _json.print("dth1", controls->getDTheta1());
 //    _json.sepItem();
-//    _json.print("dth1", controls->getDTheta1());
-    _json.sepItem();
-    _json.print("th2", controls->getTheta2());
+    _json.print("th2", _controls->getTheta2());
 //    _json.sepItem();
 //    _json.print("dth2", controls->getDTheta2());
     _json.close();
@@ -99,6 +115,10 @@
 //    _pc.printf("%s\n", outbuf);
 }
 
+Serial* Comm::getPC(){
+    return &_pc;
+}
+
 void Comm::throwNotEnoughArgsError(){
     _json.open();
     _json.print("error", "not enough input arguments");